From 73209ee4e2a40edaa44a3038e8f4f9b80ae3948a Mon Sep 17 00:00:00 2001
From: Jeff Hoefs <jeff.hoefs@gmail.com>
Date: Sun, 21 Jun 2015 21:17:09 -0700
Subject: [PATCH 01/10] initial implementation of Serial protocol

---
 examples/StandardFirmata/StandardFirmata.ino | 285 +++++++++++++++++++
 1 file changed, 285 insertions(+)

diff --git a/examples/StandardFirmata/StandardFirmata.ino b/examples/StandardFirmata/StandardFirmata.ino
index 6f882693..6eb597c2 100644
--- a/examples/StandardFirmata/StandardFirmata.ino
+++ b/examples/StandardFirmata/StandardFirmata.ino
@@ -25,8 +25,38 @@
 
 #include <Servo.h>
 #include <Wire.h>
+// SoftwareSerial is only supported for AVR-based boards
+#if defined(ARDUINO_ARCH_AVR)
+#include <SoftwareSerial.h>
+#endif
+#include <ArduinoUnit.h>
 #include <Firmata.h>
 
+#define HW_SERIAL0                  0x00
+#define HW_SERIAL1                  0x01
+#define HW_SERIAL2                  0x02
+#define HW_SERIAL3                  0x03
+
+#define SW_SERIAL0                  0x08
+#define SW_SERIAL1                  0x09
+#define SW_SERIAL2                  0x0A
+#define SW_SERIAL3                  0x0B
+
+#define SERIAL_MESSAGE              0x60
+#define SERIAL_CONFIG               0x10
+#define SERIAL_WRITE                0x20
+#define SERIAL_READ                 0x30
+#define SERIAL_REPLY                0x40
+#define SERIAL_CLOSE                0x50
+#define SERIAL_FLUSH                0x60
+#define SERIAL_LISTEN               0x70
+#define SERIAL_READ_CONTINUOUSLY    0x00
+#define SERIAL_STOP_READING         0x01
+#define SERIAL_MODE_MASK            0xF0
+#define SERIAL_PORT_ID_MASK         0x0F
+#define MAX_SERIAL_PORTS            8
+
+
 #define I2C_WRITE                   B00000000
 #define I2C_READ                    B00001000
 #define I2C_READ_CONTINUOUSLY       B00010000
@@ -61,6 +91,15 @@ unsigned long currentMillis;        // store the current value from millis()
 unsigned long previousMillis;       // for comparison with currentMillis
 unsigned int samplingInterval = 19; // how often to run the main loop (in ms)
 
+/* serial message */
+Stream *swSerial0 = NULL;
+Stream *swSerial1 = NULL;
+Stream *swSerial2 = NULL;
+Stream *swSerial3 = NULL;
+
+byte reportSerial[MAX_SERIAL_PORTS];
+signed char serialIndex = -1;
+
 /* i2c data */
 struct i2c_device_info {
   byte addr;
@@ -85,6 +124,10 @@ byte servoCount = 0;
 
 boolean isResetting = false;
 
+int memCheckCounter = 0;
+char buffer[20];
+char debugBuffer[50];
+
 /* utility functions */
 void wireWrite(byte data)
 {
@@ -108,6 +151,90 @@ byte wireRead(void)
  * FUNCTIONS
  *============================================================================*/
 
+// get a pointer to the serial port associated with the specified port id
+Stream* getPortFromId(byte portId)
+{
+  switch (portId) {
+    case HW_SERIAL0:
+      // block use of Serial (typically pins 0 and 1) until ability to reclaim Serial is implemented
+      //return &Serial;
+      return NULL;
+#if defined(UBRR1H) || defined(USBCON)
+    case HW_SERIAL1:
+      return &Serial1;
+#endif
+#if defined(UBRR2H) || defined(SERIAL_PORT_HARDWARE2)
+    case HW_SERIAL2:
+      return &Serial2;
+#endif
+#if defined(UBRR3H) || defined(SERIAL_PORT_HARDWARE3)
+    case HW_SERIAL3:
+      return &Serial3;
+#endif
+#if defined(ARDUINO_ARCH_AVR)
+    case SW_SERIAL0:
+      if (swSerial0 != NULL) {
+        // instances of SoftwareSerial are already pointers so simply return the instance
+        return swSerial0;
+      }
+      break;
+    case SW_SERIAL1:
+      if (swSerial1 != NULL) {
+        return swSerial1;
+      }
+      break;
+    case SW_SERIAL2:
+      if (swSerial2 != NULL) {
+        return swSerial2;
+      }
+      break;
+    case SW_SERIAL3:
+      if (swSerial3 != NULL) {
+        return swSerial3;
+      }
+      break;
+#endif
+  }
+  return NULL;
+}
+
+// Check serial ports that have READ_CONTINUOUS mode set and relay any data
+// for each port to the device attached to that port.
+void checkSerial()
+{
+  byte portId, serialData;
+  Stream* serialPort;
+
+  if (serialIndex > -1) {
+
+    // loop through all reporting (READ_CONTINUOUS) serial ports
+    for (byte i = 0; i < serialIndex + 1; i++) {
+      portId = reportSerial[i];
+      serialPort = getPortFromId(portId);
+      if (serialPort == NULL) {
+        continue;
+      }
+      // only the SoftwareSerial port that is "listening" can read data
+      if (portId > 7 && !((SoftwareSerial*)serialPort)->isListening()) {
+        continue;
+      }
+      if (serialPort->available() > 0) {
+        Firmata.write(START_SYSEX);
+        Firmata.write(SERIAL_MESSAGE);
+        Firmata.write(SERIAL_REPLY | portId);
+        // relay serial data to the serial device
+        while (serialPort->available() > 0) {
+          serialData = serialPort->read();
+          Firmata.write(serialData & 0x7F);
+          Firmata.write((serialData >> 7) & 0x7F);
+        }
+        Firmata.write(END_SYSEX);
+      }
+
+    }
+  }
+}
+
 void attachServo(byte pin, int minPulse, int maxPulse)
 {
   if (servoCount < MAX_SERVOS) {
@@ -586,6 +713,137 @@ void sysexCallback(byte command, byte argc, byte *argv)
       }
       Firmata.write(END_SYSEX);
       break;
+
+    case SERIAL_MESSAGE:
+      Stream * serialPort;
+      mode = argv[0] & SERIAL_MODE_MASK;
+      byte portId = argv[0] & SERIAL_PORT_ID_MASK;
+
+      switch (mode) {
+        case SERIAL_CONFIG:
+          {
+            long baud = (long)argv[1] | ((long)argv[2] << 7) | ((long)argv[3] << 14);
+            int bytesToRead = (int)argv[4] | ((int)argv[5] << 7); // not yet used
+            byte txPin, rxPin;
+
+            if (portId > 7 && argc > 6) {
+              rxPin = argv[6];
+              txPin = argv[7];
+            }
+
+            if (portId < 8) {
+              serialPort = getPortFromId(portId);
+              if (serialPort != NULL) {
+                ((HardwareSerial*)serialPort)->begin(baud);
+              }
+            } else {
+#if defined(ARDUINO_ARCH_AVR)
+              switch (portId) {
+                case SW_SERIAL0:
+                  if (swSerial0 == NULL) {
+                    swSerial0 = new SoftwareSerial(rxPin, txPin);
+                  }
+                  break;
+                case SW_SERIAL1:
+                  if (swSerial1 == NULL) {
+                    swSerial1 = new SoftwareSerial(rxPin, txPin);
+                  }
+                  break;
+                case SW_SERIAL2:
+                  if (swSerial2 == NULL) {
+                    swSerial2 = new SoftwareSerial(rxPin, txPin);
+                  }
+                  break;
+                case SW_SERIAL3:
+                  if (swSerial3 == NULL) {
+                    swSerial3 = new SoftwareSerial(rxPin, txPin);
+                  }
+                  break;
+              }
+              serialPort = getPortFromId(portId);
+              if (serialPort != NULL) {
+                ((SoftwareSerial*)serialPort)->begin(baud);
+              }
+#endif
+            }
+            break; // SERIAL_CONFIG
+          }
+        case SERIAL_WRITE:
+          {
+            byte data;
+            serialPort = getPortFromId(portId);
+            if (serialPort == NULL) {
+              break;
+            }
+            for (byte i = 1; i < argc; i += 2) {
+              data = argv[i] + (argv[i + 1] << 7);
+              serialPort->write(data);
+            }
+            break;
+          }
+        case SERIAL_READ:
+          if (argv[1] == SERIAL_READ_CONTINUOUSLY) {
+            if (serialIndex + 1 >= MAX_SERIAL_PORTS) {
+              break;
+            }
+            serialIndex++;
+            reportSerial[serialIndex] = portId;
+          } else if (argv[1] == SERIAL_STOP_READING) {
+            byte serialIndexToSkip;
+            if (serialIndex <= 0) {
+              serialIndex = -1;
+            } else {
+              for (byte i = 0; i < serialIndex + 1; i++) {
+                if (reportSerial[i] == portId) {
+                  serialIndexToSkip = i;
+                  break;
+                }
+              }
+              // shift elements over to fill space left by removed element
+              for (byte i = serialIndexToSkip; i < serialIndex + 1; i++) {
+                if (i < MAX_SERIAL_PORTS) {
+                  reportSerial[i] = reportSerial[i + 1];
+                }
+              }
+              serialIndex--;
+            }
+          }
+          break;
+        case SERIAL_CLOSE:
+          serialPort = getPortFromId(portId);
+          if (serialPort != NULL) {
+            if (portId < 8) {
+              ((HardwareSerial*)serialPort)->end();
+            } else {
+#if defined(ARDUINO_ARCH_AVR)
+              ((SoftwareSerial*)serialPort)->end();
+              if (serialPort != NULL) {
+                free(serialPort);
+                serialPort = NULL;
+              }
+#endif
+            }
+          }
+          break;
+        case SERIAL_FLUSH:
+          serialPort = getPortFromId(portId);
+          if (serialPort != NULL) {
+            getPortFromId(portId)->flush();
+          }
+          break;
+#if defined(ARDUINO_ARCH_AVR)
+        case SERIAL_LISTEN:
+          // can only call listen() on software serial ports
+          if (portId > 7) {
+            serialPort = getPortFromId(portId);
+            if (serialPort != NULL) {
+              ((SoftwareSerial*)serialPort)->listen();
+            }
+          }
+          break;
+#endif
+      }
+      break;
   }
 }
 
@@ -619,6 +877,7 @@ void disableI2CPins() {
 
 void systemResetCallback()
 {
+  Stream *serialPort;
   isResetting = true;
 
   // initialize a defalt state
@@ -628,6 +887,18 @@ void systemResetCallback()
     disableI2CPins();
   }
 
+#if defined(ARDUINO_ARCH_AVR)
+  // free memory allocated for SoftwareSerial ports
+  for (byte i = SW_SERIAL0; i < SW_SERIAL3 + 1; i++) {
+    serialPort = getPortFromId(i);
+    if (serialPort != NULL) {
+      free(serialPort);
+      serialPort = NULL;
+    }
+  }
+  serialIndex = -1;
+#endif
+
   for (byte i = 0; i < TOTAL_PORTS; i++) {
     reportPINs[i] = false;    // by default, reporting off
     portConfigInputs[i] = 0;  // until activated
@@ -726,5 +997,19 @@ void loop()
         readAndReportData(query[i].addr, query[i].reg, query[i].bytes);
       }
     }
+
+    //    // check for memory leaks
+    //    // send memory reading approximately ever 10 seconds
+    //    // 526 * 19 (default sampling interval) = 9994 ms
+    //    // increase 526 for long running processes
+    //    if (memCheckCounter++ == 526) {
+    //      sprintf(buffer, "%u", freeMemory());
+    //      Firmata.sendString(buffer);
+    //      memCheckCounter = 0;
+    //    }
+
   }
+
+  // TODO - figure out best location to call this function.
+  checkSerial();
 }

From d90e982a968cca3c23c493b87ed00a8b942f31fc Mon Sep 17 00:00:00 2001
From: Jeff Hoefs <jeff.hoefs@gmail.com>
Date: Mon, 22 Jun 2015 23:54:40 -0700
Subject: [PATCH 02/10] always reset serialIndex

---
 examples/StandardFirmata/StandardFirmata.ino | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/StandardFirmata/StandardFirmata.ino b/examples/StandardFirmata/StandardFirmata.ino
index 6eb597c2..98159117 100644
--- a/examples/StandardFirmata/StandardFirmata.ino
+++ b/examples/StandardFirmata/StandardFirmata.ino
@@ -896,8 +896,8 @@ void systemResetCallback()
       serialPort = NULL;
     }
   }
-  serialIndex = -1;
 #endif
+  serialIndex = -1;
 
   for (byte i = 0; i < TOTAL_PORTS; i++) {
     reportPINs[i] = false;    // by default, reporting off

From 580a83297c97f2af3b3143413dc1cd1a1d2ecb2a Mon Sep 17 00:00:00 2001
From: Jeff Hoefs <jeff.hoefs@gmail.com>
Date: Tue, 4 Aug 2015 21:42:19 -0700
Subject: [PATCH 03/10] add missing directive

---
 examples/StandardFirmata/StandardFirmata.ino | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/examples/StandardFirmata/StandardFirmata.ino b/examples/StandardFirmata/StandardFirmata.ino
index 98159117..529c9f3f 100644
--- a/examples/StandardFirmata/StandardFirmata.ino
+++ b/examples/StandardFirmata/StandardFirmata.ino
@@ -214,10 +214,12 @@ void checkSerial()
       if (serialPort == NULL) {
         continue;
       }
+#if defined(ARDUINO_ARCH_AVR)
       // only the SoftwareSerial port that is "listening" can read data
       if (portId > 7 && !((SoftwareSerial*)serialPort)->isListening()) {
         continue;
       }
+#endif
       if (serialPort->available() > 0) {
         Firmata.write(START_SYSEX);
         Firmata.write(SERIAL_MESSAGE);

From 5b8bff728784c7faa6b65885b48079ba7c967969 Mon Sep 17 00:00:00 2001
From: Jeff Hoefs <jeff.hoefs@gmail.com>
Date: Sun, 16 Aug 2015 00:08:07 -0700
Subject: [PATCH 04/10] implement bytesToRead from Serial protocol

---
 examples/StandardFirmata/StandardFirmata.ino | 17 +++++++++++++++--
 1 file changed, 15 insertions(+), 2 deletions(-)

diff --git a/examples/StandardFirmata/StandardFirmata.ino b/examples/StandardFirmata/StandardFirmata.ino
index 529c9f3f..fe735c3b 100644
--- a/examples/StandardFirmata/StandardFirmata.ino
+++ b/examples/StandardFirmata/StandardFirmata.ino
@@ -98,6 +98,7 @@ Stream *swSerial2 = NULL;
 Stream *swSerial3 = NULL;
 
 byte reportSerial[MAX_SERIAL_PORTS];
+int serialBytesToRead[12];
 signed char serialIndex = -1;
 
 /* i2c data */
@@ -203,6 +204,8 @@ Stream* getPortFromId(byte portId)
 void checkSerial()
 {
   byte portId, serialData;
+  int bytesToRead = 0;
+  int numBytesToRead = 0;
   Stream* serialPort;
 
   if (serialIndex > -1) {
@@ -210,6 +213,7 @@ void checkSerial()
     // loop through all reporting (READ_CONTINUOUS) serial ports
     for (byte i = 0; i < serialIndex + 1; i++) {
       portId = reportSerial[i];
+      bytesToRead = serialBytesToRead[portId];
       serialPort = getPortFromId(portId);
       if (serialPort == NULL) {
         continue;
@@ -224,11 +228,19 @@ void checkSerial()
         Firmata.write(START_SYSEX);
         Firmata.write(SERIAL_MESSAGE);
         Firmata.write(SERIAL_REPLY | portId);
+
+        if (bytesToRead == 0 || (serialPort->available() <= bytesToRead)) {
+          numBytesToRead = serialPort->available();
+        } else {
+          numBytesToRead = bytesToRead;
+        }
+
         // relay serial data to the serial device
-        while (serialPort->available() > 0) {
+        while (numBytesToRead > 0) {
           serialData = serialPort->read();
           Firmata.write(serialData & 0x7F);
           Firmata.write((serialData >> 7) & 0x7F);
+          numBytesToRead--;
         }
         Firmata.write(END_SYSEX);
       }
@@ -725,9 +737,10 @@ void sysexCallback(byte command, byte argc, byte *argv)
         case SERIAL_CONFIG:
           {
             long baud = (long)argv[1] | ((long)argv[2] << 7) | ((long)argv[3] << 14);
-            int bytesToRead = (int)argv[4] | ((int)argv[5] << 7); // not yet used
             byte txPin, rxPin;
 
+            serialBytesToRead[portId] = (int)argv[4] | ((int)argv[5] << 7);
+
             if (portId > 7 && argc > 6) {
               rxPin = argv[6];
               txPin = argv[7];

From 31202222ea81e3c1e62b05b3e46ad51fdc89172a Mon Sep 17 00:00:00 2001
From: Jeff Hoefs <jeff.hoefs@gmail.com>
Date: Sun, 23 Aug 2015 01:35:45 -0700
Subject: [PATCH 05/10] add HW pin constants and serial pin config

---
 Boards.h                                     | 43 ++++++++-
 Firmata.h                                    |  4 +-
 examples/StandardFirmata/StandardFirmata.ino | 93 +++++++++++++++++---
 3 files changed, 126 insertions(+), 14 deletions(-)

diff --git a/Boards.h b/Boards.h
index 2f78675b..a082b205 100644
--- a/Boards.h
+++ b/Boards.h
@@ -200,12 +200,19 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 #define TOTAL_ANALOG_PINS       16
 #define TOTAL_PINS              70 // 54 digital + 16 analog
 #define VERSION_BLINK_PIN       13
+#define PIN_SERIAL1_RX          19
+#define PIN_SERIAL1_TX          18
+#define PIN_SERIAL2_RX          17
+#define PIN_SERIAL2_TX          16
+#define PIN_SERIAL3_RX          15
+#define PIN_SERIAL3_TX          14
 #define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) < TOTAL_PINS)
 #define IS_PIN_ANALOG(p)        ((p) >= 54 && (p) < TOTAL_PINS)
 #define IS_PIN_PWM(p)           digitalPinHasPWM(p)
 #define IS_PIN_SERVO(p)         ((p) >= 2 && (p) - 2 < MAX_SERVOS)
 #define IS_PIN_I2C(p)           ((p) == 20 || (p) == 21)
 #define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) > 13 && (p) < 20)
 #define PIN_TO_DIGITAL(p)       (p)
 #define PIN_TO_ANALOG(p)        ((p) - 54)
 #define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
@@ -217,11 +224,18 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 #define TOTAL_ANALOG_PINS       12
 #define TOTAL_PINS              66 // 54 digital + 12 analog
 #define VERSION_BLINK_PIN       13
+#define PIN_SERIAL1_RX          19
+#define PIN_SERIAL1_TX          18
+#define PIN_SERIAL2_RX          17
+#define PIN_SERIAL2_TX          16
+#define PIN_SERIAL3_RX          15
+#define PIN_SERIAL3_TX          14
 #define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) < TOTAL_PINS)
 #define IS_PIN_ANALOG(p)        ((p) >= 54 && (p) < TOTAL_PINS)
 #define IS_PIN_PWM(p)           digitalPinHasPWM(p)
 #define IS_PIN_SERVO(p)         ((p) >= 2 && (p) - 2 < MAX_SERVOS)
 #define IS_PIN_I2C(p)           ((p) == 20 || (p) == 21) // 70 71
+#define IS_PIN_SERIAL(p)        ((p) > 13 && (p) < 20)
 #define PIN_TO_DIGITAL(p)       (p)
 #define PIN_TO_ANALOG(p)        ((p) - 54)
 #define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
@@ -233,12 +247,15 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 #define TOTAL_ANALOG_PINS       0
 #define TOTAL_PINS              21 // 21 digital + no analog
 #define VERSION_BLINK_PIN       6
+#define PIN_SERIAL1_RX          2
+#define PIN_SERIAL1_TX          3
 #define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < TOTAL_PINS)
 #define IS_PIN_ANALOG(p)        (0)
 #define IS_PIN_PWM(p)           digitalPinHasPWM(p)
 #define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
 #define IS_PIN_I2C(p)           (0)
 #define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) == 2 || (p) == 3)
 #define PIN_TO_DIGITAL(p)       (p)
 #define PIN_TO_ANALOG(p)        (0)
 #define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
@@ -250,12 +267,15 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 #define TOTAL_ANALOG_PINS       12
 #define TOTAL_PINS              25 // 11 digital + 12 analog
 #define VERSION_BLINK_PIN       11
+#define PIN_SERIAL1_RX          7
+#define PIN_SERIAL1_TX          8
 #define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < TOTAL_PINS)
 #define IS_PIN_ANALOG(p)        ((p) >= 11 && (p) <= 22)
 #define IS_PIN_PWM(p)           digitalPinHasPWM(p)
 #define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
 #define IS_PIN_I2C(p)           ((p) == 5 || (p) == 6)
 #define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) == 7 || (p) == 8)
 #define PIN_TO_DIGITAL(p)       (p)
 #define PIN_TO_ANALOG(p)        (((p)<22)?21-(p):11)
 #define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
@@ -267,11 +287,18 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 #define TOTAL_ANALOG_PINS       14
 #define TOTAL_PINS              38 // 24 digital + 10 analog-digital + 4 analog
 #define VERSION_BLINK_PIN       13
+#define PIN_SERIAL1_RX          0
+#define PIN_SERIAL1_TX          1
+#define PIN_SERIAL2_RX          9
+#define PIN_SERIAL2_TX          10
+#define PIN_SERIAL3_RX          7
+#define PIN_SERIAL3_TX          8
 #define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) <= 34)
 #define IS_PIN_ANALOG(p)        (((p) >= 14 && (p) <= 23) || ((p) >= 34 && (p) <= 38))
 #define IS_PIN_PWM(p)           digitalPinHasPWM(p)
 #define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
 #define IS_PIN_I2C(p)           ((p) == 18 || (p) == 19)
+#define IS_PIN_SERIAL(p)        (((p) > 6 && (p) < 11) || ((p) == 0 || (p) == 1))
 #define PIN_TO_DIGITAL(p)       (p)
 #define PIN_TO_ANALOG(p)        (((p)<=23)?(p)-14:(p)-24)
 #define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
@@ -283,12 +310,15 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 #define TOTAL_ANALOG_PINS       8
 #define TOTAL_PINS              46 // 38 digital + 8 analog
 #define VERSION_BLINK_PIN       6
+#define PIN_SERIAL1_RX          2
+#define PIN_SERIAL1_TX          3
 #define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < TOTAL_PINS)
 #define IS_PIN_ANALOG(p)        ((p) >= 38 && (p) < TOTAL_PINS)
 #define IS_PIN_PWM(p)           digitalPinHasPWM(p)
 #define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
 #define IS_PIN_I2C(p)           ((p) == 0 || (p) == 1)
 #define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) == 2 || (p) == 3)
 #define PIN_TO_DIGITAL(p)       (p)
 #define PIN_TO_ANALOG(p)        ((p) - 38)
 #define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
@@ -300,28 +330,35 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 #define TOTAL_ANALOG_PINS       12
 #define TOTAL_PINS              30 // 14 digital + 12 analog + 4 SPI (D14-D17 on ISP header)
 #define VERSION_BLINK_PIN       13
+#define PIN_SERIAL1_RX          0
+#define PIN_SERIAL1_TX          1
 #define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < TOTAL_PINS)
 #define IS_PIN_ANALOG(p)        ((p) >= 18 && (p) < TOTAL_PINS)
 #define IS_PIN_PWM(p)           ((p) == 3 || (p) == 5 || (p) == 6 || (p) == 9 || (p) == 10 || (p) == 11 || (p) == 13)
 #define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
 #define IS_PIN_I2C(p)           ((p) == 2 || (p) == 3)
 #define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) == 0 || (p) == 1)
 #define PIN_TO_DIGITAL(p)       (p)
 #define PIN_TO_ANALOG(p)        (p) - 18
 #define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
 #define PIN_TO_SERVO(p)         (p)
 
+
 // Intel Galileo Board
 #elif defined(ARDUINO_LINUX)
 #define TOTAL_ANALOG_PINS       6
 #define TOTAL_PINS              20 // 14 digital + 6 analog
 #define VERSION_BLINK_PIN       13
+#define PIN_SERIAL1_RX          0
+#define PIN_SERIAL1_TX          1
 #define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) <= 19)
 #define IS_PIN_ANALOG(p)        ((p) >= 14 && (p) <= 19)
 #define IS_PIN_PWM(p)           digitalPinHasPWM(p)
 #define IS_PIN_SERVO(p)         (IS_PIN_DIGITAL(p) && (p) - 2 < MAX_SERVOS)
 #define IS_PIN_I2C(p)           ((p) == SDA || (p) == SCL)
 #define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) == 0 || (p) == 1)
 #define PIN_TO_DIGITAL(p)       (p)
 #define PIN_TO_ANALOG(p)        ((p) - 14)
 #define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
@@ -490,7 +527,7 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 // Pic32 chipKIT MAX32
 #elif defined(_BOARD_MEGA_) && defined(__PIC32)  // NOTE: no _BOARD_MAX32_ to use
 #define TOTAL_ANALOG_PINS       NUM_ANALOG_PINS  // 16
-#define TOTAL_PINS              NUM_DIGITAL_PINS // 87 
+#define TOTAL_PINS              NUM_DIGITAL_PINS // 87
 #define MAX_SERVOS              NUM_DIGITAL_PINS
 #define VERSION_BLINK_PIN       PIN_LED1
 #define IS_PIN_DIGITAL(p)       ((p) >= 2)
@@ -534,6 +571,10 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 #define IS_PIN_SPI(p)           0
 #endif
 
+#ifndef IS_PIN_SERIAL
+#define IS_PIN_SERIAL(p)        0
+#endif
+
 /*==============================================================================
  * readPort() - Read an 8 bit port
  *============================================================================*/
diff --git a/Firmata.h b/Firmata.h
index 5b8a3f44..8ad832ee 100644
--- a/Firmata.h
+++ b/Firmata.h
@@ -41,6 +41,7 @@
 
 // extended command set using sysex (0-127/0x00-0x7F)
 /* 0x00-0x0F reserved for user-defined commands */
+#define SERIAL_MESSAGE          0x60 // communicate with serial devices, including other boards
 #define ENCODER_DATA            0x61 // reply with encoders current positions
 #define SERVO_CONFIG            0x70 // set max angle, minPulse, maxPulse, freq
 #define STRING_DATA             0x71 // a string message with 14-bits per char
@@ -79,8 +80,9 @@
 #define ONEWIRE                 0x07 // pin configured for 1-wire
 #define STEPPER                 0x08 // pin configured for stepper motor
 #define ENCODER                 0x09 // pin configured for rotary encoders
+#define MODE_SERIAL             0x0A // pin configured for serial communication
 #define IGNORE                  0x7F // pin configured to be ignored by digitalWrite and capabilityResponse
-#define TOTAL_PIN_MODES         11
+#define TOTAL_PIN_MODES         12
 
 extern "C" {
   // callback function types
diff --git a/examples/StandardFirmata/StandardFirmata.ino b/examples/StandardFirmata/StandardFirmata.ino
index fe735c3b..e88142f7 100644
--- a/examples/StandardFirmata/StandardFirmata.ino
+++ b/examples/StandardFirmata/StandardFirmata.ino
@@ -42,7 +42,14 @@
 #define SW_SERIAL2                  0x0A
 #define SW_SERIAL3                  0x0B
 
-#define SERIAL_MESSAGE              0x60
+// map configuration query response resolution value to serial pin type
+#define CONFIG_RX1                  0x02
+#define CONFIG_TX1                  0x03
+#define CONFIG_RX2                  0x04
+#define CONFIG_TX2                  0x05
+#define CONFIG_RX3                  0x06
+#define CONFIG_TX3                  0x07
+
 #define SERIAL_CONFIG               0x10
 #define SERIAL_WRITE                0x20
 #define SERIAL_READ                 0x30
@@ -67,7 +74,7 @@
 #define I2C_REGISTER_NOT_SPECIFIED  -1
 
 // the minimum interval for sampling analog input
-#define MINIMUM_SAMPLING_INTERVAL 10
+#define MINIMUM_SAMPLING_INTERVAL   10
 
 
 /*==============================================================================
@@ -160,19 +167,19 @@ Stream* getPortFromId(byte portId)
       // block use of Serial (typically pins 0 and 1) until ability to reclaim Serial is implemented
       //return &Serial;
       return NULL;
-#if defined(UBRR1H) || defined(USBCON)
+#if defined(PIN_SERIAL1_RX)
     case HW_SERIAL1:
       return &Serial1;
 #endif
-#if defined(UBRR2H) || defined(SERIAL_PORT_HARDWARE2)
+#if defined(PIN_SERIAL2_RX)
     case HW_SERIAL2:
       return &Serial2;
 #endif
-#if defined(UBRR3H) || defined(SERIAL_PORT_HARDWARE3)
+#if defined(PIN_SERIAL3_RX)
     case HW_SERIAL3:
       return &Serial3;
 #endif
-#if defined(ARDUINO_ARCH_AVR)
+#if defined(SoftwareSerial_h)
     case SW_SERIAL0:
       if (swSerial0 != NULL) {
         // instances of SoftwareSerial are already pointers so simply return the instance
@@ -218,7 +225,7 @@ void checkSerial()
       if (serialPort == NULL) {
         continue;
       }
-#if defined(ARDUINO_ARCH_AVR)
+#if defined(SoftwareSerial_h)
       // only the SoftwareSerial port that is "listening" can read data
       if (portId > 7 && !((SoftwareSerial*)serialPort)->isListening()) {
         continue;
@@ -249,6 +256,57 @@ void checkSerial()
   }
 }
 
+/*
+ * Return the serial serial pin type (RX1, TX1, RX2, TX2, etc) for the specified pin
+ */
+byte getSerialPinType(byte pin) {
+#if defined(PIN_SERIAL1_RX)
+  if (pin == PIN_SERIAL1_RX) return CONFIG_RX1;
+  if (pin == PIN_SERIAL1_TX) return CONFIG_TX1;
+#endif
+#if defined(PIN_SERIAL2_RX)
+  if (pin == PIN_SERIAL2_RX) return CONFIG_RX2;
+  if (pin == PIN_SERIAL2_TX) return CONFIG_TX2;
+#endif
+#if defined(PIN_SERIAL3_RX)
+  if (pin == PIN_SERIAL3_RX) return CONFIG_RX3;
+  if (pin == PIN_SERIAL3_TX) return CONFIG_TX3;
+#endif
+  return 0;
+}
+
+byte configHWSerialPins(byte portId) {
+  byte rxPin, txPin;
+  switch (portId) {
+#if defined(PIN_SERIAL1_RX)
+    case HW_SERIAL1:
+      rxPin = PIN_SERIAL1_RX;
+      txPin = PIN_SERIAL1_TX;
+    break;
+#endif
+#if defined(PIN_SERIAL2_RX)
+    case HW_SERIAL2:
+      rxPin = PIN_SERIAL2_RX;
+      txPin = PIN_SERIAL2_TX;
+    break;
+#endif
+#if defined(PIN_SERIAL3_RX)
+    case HW_SERIAL3:
+      rxPin = PIN_SERIAL3_RX;
+      txPin = PIN_SERIAL3_TX;
+    break;
+#endif
+    default:
+      return 0;
+  }
+  setPinModeCallback(rxPin, MODE_SERIAL);
+  setPinModeCallback(txPin, MODE_SERIAL);
+
+  // Fixes an issue where some serial devices would not work properly with Arduino Due
+  // because all Arduino pins are set to OUTPUT by default in StandardFirmata.
+  pinMode(rxPin, INPUT);
+}
+
 void attachServo(byte pin, int minPulse, int maxPulse)
 {
   if (servoCount < MAX_SERVOS) {
@@ -439,6 +497,10 @@ void setPinModeCallback(byte pin, int mode)
         pinConfig[pin] = I2C;
       }
       break;
+    case MODE_SERIAL:
+      // used for both HW and SW serial
+      pinConfig[pin] = MODE_SERIAL;
+      break;
     default:
       Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
   }
@@ -700,6 +762,10 @@ void sysexCallback(byte command, byte argc, byte *argv)
           Firmata.write(I2C);
           Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
         }
+        if (IS_PIN_SERIAL(pin)) {
+          Firmata.write(MODE_SERIAL);
+          Firmata.write(getSerialPinType(pin));
+        }
         Firmata.write(127);
       }
       Firmata.write(END_SYSEX);
@@ -749,10 +815,11 @@ void sysexCallback(byte command, byte argc, byte *argv)
             if (portId < 8) {
               serialPort = getPortFromId(portId);
               if (serialPort != NULL) {
+                configHWSerialPins(portId);
                 ((HardwareSerial*)serialPort)->begin(baud);
               }
             } else {
-#if defined(ARDUINO_ARCH_AVR)
+#if defined(SoftwareSerial_h)
               switch (portId) {
                 case SW_SERIAL0:
                   if (swSerial0 == NULL) {
@@ -777,6 +844,8 @@ void sysexCallback(byte command, byte argc, byte *argv)
               }
               serialPort = getPortFromId(portId);
               if (serialPort != NULL) {
+                setPinModeCallback(rxPin, MODE_SERIAL);
+                setPinModeCallback(txPin, MODE_SERIAL);
                 ((SoftwareSerial*)serialPort)->begin(baud);
               }
 #endif
@@ -830,7 +899,7 @@ void sysexCallback(byte command, byte argc, byte *argv)
             if (portId < 8) {
               ((HardwareSerial*)serialPort)->end();
             } else {
-#if defined(ARDUINO_ARCH_AVR)
+#if defined(SoftwareSerial_h)
               ((SoftwareSerial*)serialPort)->end();
               if (serialPort != NULL) {
                 free(serialPort);
@@ -846,7 +915,7 @@ void sysexCallback(byte command, byte argc, byte *argv)
             getPortFromId(portId)->flush();
           }
           break;
-#if defined(ARDUINO_ARCH_AVR)
+#if defined(SoftwareSerial_h)
         case SERIAL_LISTEN:
           // can only call listen() on software serial ports
           if (portId > 7) {
@@ -902,7 +971,7 @@ void systemResetCallback()
     disableI2CPins();
   }
 
-#if defined(ARDUINO_ARCH_AVR)
+#if defined(SoftwareSerial_h)
   // free memory allocated for SoftwareSerial ports
   for (byte i = SW_SERIAL0; i < SW_SERIAL3 + 1; i++) {
     serialPort = getPortFromId(i);
@@ -967,7 +1036,7 @@ void setup()
   // Call begin(baud) on the alternate serial port and pass it to Firmata to begin like this:
   // Serial1.begin(57600);
   // Firmata.begin(Serial1);
-  // then comment out or remove lines 701 - 704 below
+  // However do not do this if you are using SERIAL_MESSAGE
 
   Firmata.begin(57600);
   while (!Serial) {

From 7e7118c7f102c34c2800ceae595cda03e799cc7e Mon Sep 17 00:00:00 2001
From: Jeff Hoefs <jeff.hoefs@gmail.com>
Date: Sun, 23 Aug 2015 22:16:40 -0700
Subject: [PATCH 06/10] move serial defines and utility functions to utility
 file

---
 examples/StandardFirmata/StandardFirmata.ino | 110 +++--------------
 utility/serialUtils.h                        | 117 +++++++++++++++++++
 2 files changed, 135 insertions(+), 92 deletions(-)
 create mode 100644 utility/serialUtils.h

diff --git a/examples/StandardFirmata/StandardFirmata.ino b/examples/StandardFirmata/StandardFirmata.ino
index e88142f7..7f1f2f72 100644
--- a/examples/StandardFirmata/StandardFirmata.ino
+++ b/examples/StandardFirmata/StandardFirmata.ino
@@ -25,44 +25,14 @@
 
 #include <Servo.h>
 #include <Wire.h>
+#include <ArduinoUnit.h>
+#include <Firmata.h>
+
 // SoftwareSerial is only supported for AVR-based boards
 #if defined(ARDUINO_ARCH_AVR)
 #include <SoftwareSerial.h>
 #endif
-#include <ArduinoUnit.h>
-#include <Firmata.h>
-
-#define HW_SERIAL0                  0x00
-#define HW_SERIAL1                  0x01
-#define HW_SERIAL2                  0x02
-#define HW_SERIAL3                  0x03
-
-#define SW_SERIAL0                  0x08
-#define SW_SERIAL1                  0x09
-#define SW_SERIAL2                  0x0A
-#define SW_SERIAL3                  0x0B
-
-// map configuration query response resolution value to serial pin type
-#define CONFIG_RX1                  0x02
-#define CONFIG_TX1                  0x03
-#define CONFIG_RX2                  0x04
-#define CONFIG_TX2                  0x05
-#define CONFIG_RX3                  0x06
-#define CONFIG_TX3                  0x07
-
-#define SERIAL_CONFIG               0x10
-#define SERIAL_WRITE                0x20
-#define SERIAL_READ                 0x30
-#define SERIAL_REPLY                0x40
-#define SERIAL_CLOSE                0x50
-#define SERIAL_FLUSH                0x60
-#define SERIAL_LISTEN               0x70
-#define SERIAL_READ_CONTINUOUSLY    0x00
-#define SERIAL_STOP_READING         0x01
-#define SERIAL_MODE_MASK            0xF0
-#define SERIAL_PORT_ID_MASK         0x0F
-#define MAX_SERIAL_PORTS            8
-
+#include "utility/serialUtils.h"
 
 #define I2C_WRITE                   B00000000
 #define I2C_READ                    B00001000
@@ -134,7 +104,6 @@ boolean isResetting = false;
 
 int memCheckCounter = 0;
 char buffer[20];
-char debugBuffer[50];
 
 /* utility functions */
 void wireWrite(byte data)
@@ -256,57 +225,6 @@ void checkSerial()
   }
 }
 
-/*
- * Return the serial serial pin type (RX1, TX1, RX2, TX2, etc) for the specified pin
- */
-byte getSerialPinType(byte pin) {
-#if defined(PIN_SERIAL1_RX)
-  if (pin == PIN_SERIAL1_RX) return CONFIG_RX1;
-  if (pin == PIN_SERIAL1_TX) return CONFIG_TX1;
-#endif
-#if defined(PIN_SERIAL2_RX)
-  if (pin == PIN_SERIAL2_RX) return CONFIG_RX2;
-  if (pin == PIN_SERIAL2_TX) return CONFIG_TX2;
-#endif
-#if defined(PIN_SERIAL3_RX)
-  if (pin == PIN_SERIAL3_RX) return CONFIG_RX3;
-  if (pin == PIN_SERIAL3_TX) return CONFIG_TX3;
-#endif
-  return 0;
-}
-
-byte configHWSerialPins(byte portId) {
-  byte rxPin, txPin;
-  switch (portId) {
-#if defined(PIN_SERIAL1_RX)
-    case HW_SERIAL1:
-      rxPin = PIN_SERIAL1_RX;
-      txPin = PIN_SERIAL1_TX;
-    break;
-#endif
-#if defined(PIN_SERIAL2_RX)
-    case HW_SERIAL2:
-      rxPin = PIN_SERIAL2_RX;
-      txPin = PIN_SERIAL2_TX;
-    break;
-#endif
-#if defined(PIN_SERIAL3_RX)
-    case HW_SERIAL3:
-      rxPin = PIN_SERIAL3_RX;
-      txPin = PIN_SERIAL3_TX;
-    break;
-#endif
-    default:
-      return 0;
-  }
-  setPinModeCallback(rxPin, MODE_SERIAL);
-  setPinModeCallback(txPin, MODE_SERIAL);
-
-  // Fixes an issue where some serial devices would not work properly with Arduino Due
-  // because all Arduino pins are set to OUTPUT by default in StandardFirmata.
-  pinMode(rxPin, INPUT);
-}
-
 void attachServo(byte pin, int minPulse, int maxPulse)
 {
   if (servoCount < MAX_SERVOS) {
@@ -804,6 +722,7 @@ void sysexCallback(byte command, byte argc, byte *argv)
           {
             long baud = (long)argv[1] | ((long)argv[2] << 7) | ((long)argv[3] << 14);
             byte txPin, rxPin;
+            serial_pins pins;
 
             serialBytesToRead[portId] = (int)argv[4] | ((int)argv[5] << 7);
 
@@ -815,7 +734,14 @@ void sysexCallback(byte command, byte argc, byte *argv)
             if (portId < 8) {
               serialPort = getPortFromId(portId);
               if (serialPort != NULL) {
-                configHWSerialPins(portId);
+                pins = getSerialPinNumbers(portId);
+                if (pins.rx != 0 && pins.tx != 0) {
+                  setPinModeCallback(pins.rx, MODE_SERIAL);
+                  setPinModeCallback(pins.tx, MODE_SERIAL);
+                  // Fixes an issue where some serial devices would not work properly with Arduino Due
+                  // because all Arduino pins are set to OUTPUT by default in StandardFirmata.
+                  pinMode(pins.rx, INPUT);
+                }
                 ((HardwareSerial*)serialPort)->begin(baud);
               }
             } else {
@@ -863,7 +789,7 @@ void sysexCallback(byte command, byte argc, byte *argv)
               data = argv[i] + (argv[i + 1] << 7);
               serialPort->write(data);
             }
-            break;
+            break; // SERIAL_WRITE
           }
         case SERIAL_READ:
           if (argv[1] == SERIAL_READ_CONTINUOUSLY) {
@@ -892,7 +818,7 @@ void sysexCallback(byte command, byte argc, byte *argv)
               serialIndex--;
             }
           }
-          break;
+          break; // SERIAL_READ
         case SERIAL_CLOSE:
           serialPort = getPortFromId(portId);
           if (serialPort != NULL) {
@@ -908,13 +834,13 @@ void sysexCallback(byte command, byte argc, byte *argv)
 #endif
             }
           }
-          break;
+          break; // SERIAL_CLOSE
         case SERIAL_FLUSH:
           serialPort = getPortFromId(portId);
           if (serialPort != NULL) {
             getPortFromId(portId)->flush();
           }
-          break;
+          break; // SERIAL_FLUSH
 #if defined(SoftwareSerial_h)
         case SERIAL_LISTEN:
           // can only call listen() on software serial ports
@@ -924,7 +850,7 @@ void sysexCallback(byte command, byte argc, byte *argv)
               ((SoftwareSerial*)serialPort)->listen();
             }
           }
-          break;
+          break; // SERIAL_LISTEN
 #endif
       }
       break;
diff --git a/utility/serialUtils.h b/utility/serialUtils.h
new file mode 100644
index 00000000..63255059
--- /dev/null
+++ b/utility/serialUtils.h
@@ -0,0 +1,117 @@
+/*
+  serialUtils.h - Definitions and utility functions for the Serial feature.
+  Copyright (c) 2015 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last update August 23rd, 2015
+*/
+
+#ifndef SERIAL_UTILS_H
+#define SERIAL_UTILS_H
+
+// Serial port Ids
+#define HW_SERIAL0                  0x00
+#define HW_SERIAL1                  0x01
+#define HW_SERIAL2                  0x02
+#define HW_SERIAL3                  0x03
+
+#define SW_SERIAL0                  0x08
+#define SW_SERIAL1                  0x09
+#define SW_SERIAL2                  0x0A
+#define SW_SERIAL3                  0x0B
+
+#define SERIAL_PORT_ID_MASK         0x0F
+#define MAX_SERIAL_PORTS            8
+
+// map configuration query response resolution value to serial pin type
+#define RES_RX1                     0x02
+#define RES_TX1                     0x03
+#define RES_RX2                     0x04
+#define RES_TX2                     0x05
+#define RES_RX3                     0x06
+#define RES_TX3                     0x07
+
+// Serial command bytes
+#define SERIAL_CONFIG               0x10
+#define SERIAL_WRITE                0x20
+#define SERIAL_READ                 0x30
+#define SERIAL_REPLY                0x40
+#define SERIAL_CLOSE                0x50
+#define SERIAL_FLUSH                0x60
+#define SERIAL_LISTEN               0x70
+
+// Serial read modes
+#define SERIAL_READ_CONTINUOUSLY    0x00
+#define SERIAL_STOP_READING         0x01
+#define SERIAL_MODE_MASK            0xF0
+
+struct serial_pins {
+  uint8_t rx;
+  uint8_t tx;
+};
+
+/*
+ * Get the serial serial pin type (RX1, TX1, RX2, TX2, etc) for the specified pin.
+ */
+inline uint8_t getSerialPinType(uint8_t pin) {
+#if defined(PIN_SERIAL_RX)
+  // TODO when use of HW_SERIAL0 is enabled
+#endif
+#if defined(PIN_SERIAL1_RX)
+  if (pin == PIN_SERIAL1_RX) return RES_RX1;
+  if (pin == PIN_SERIAL1_TX) return RES_TX1;
+#endif
+#if defined(PIN_SERIAL2_RX)
+  if (pin == PIN_SERIAL2_RX) return RES_RX2;
+  if (pin == PIN_SERIAL2_TX) return RES_TX2;
+#endif
+#if defined(PIN_SERIAL3_RX)
+  if (pin == PIN_SERIAL3_RX) return RES_RX3;
+  if (pin == PIN_SERIAL3_TX) return RES_TX3;
+#endif
+  return 0;
+}
+
+/*
+ * Get the RX and TX pins numbers for the specified HW serial port.
+ */
+inline serial_pins getSerialPinNumbers(uint8_t portId) {
+  serial_pins pins;
+  switch (portId) {
+#if defined(PIN_SERIAL_RX)
+    // case HW_SERIAL0:
+    //   // TODO when use of HW_SERIAL0 is enabled
+    //   break;
+#endif
+#if defined(PIN_SERIAL1_RX)
+    case HW_SERIAL1:
+      pins.rx = PIN_SERIAL1_RX;
+      pins.tx = PIN_SERIAL1_TX;
+    break;
+#endif
+#if defined(PIN_SERIAL2_RX)
+    case HW_SERIAL2:
+      pins.rx = PIN_SERIAL2_RX;
+      pins.tx = PIN_SERIAL2_TX;
+    break;
+#endif
+#if defined(PIN_SERIAL3_RX)
+    case HW_SERIAL3:
+      pins.rx = PIN_SERIAL3_RX;
+      pins.tx = PIN_SERIAL3_TX;
+    break;
+#endif
+    default:
+      pins.rx = 0;
+      pins.tx = 0;
+  }
+  return pins;
+}
+
+#endif /* SERIAL_UTILS_H */

From 0e3a973877ddd44392ba24196df9d10d1434ac89 Mon Sep 17 00:00:00 2001
From: Jeff Hoefs <jeff.hoefs@gmail.com>
Date: Sat, 3 Oct 2015 15:21:53 -0700
Subject: [PATCH 07/10] move bytesToRead from config to serialRead. remove temp
 logging

---
 examples/StandardFirmata/StandardFirmata.ino | 40 +++++++++-----------
 utility/serialUtils.h                        |  3 ++
 2 files changed, 21 insertions(+), 22 deletions(-)

diff --git a/examples/StandardFirmata/StandardFirmata.ino b/examples/StandardFirmata/StandardFirmata.ino
index 7f1f2f72..7bfa2f02 100644
--- a/examples/StandardFirmata/StandardFirmata.ino
+++ b/examples/StandardFirmata/StandardFirmata.ino
@@ -20,7 +20,7 @@
 
   See file LICENSE.txt for further informations on licensing terms.
 
-  Last updated by Jeff Hoefs: August 9th, 2015
+  Last updated by Jeff Hoefs: October 3rd, 2015
 */
 
 #include <Servo.h>
@@ -75,8 +75,8 @@ Stream *swSerial2 = NULL;
 Stream *swSerial3 = NULL;
 
 byte reportSerial[MAX_SERIAL_PORTS];
-int serialBytesToRead[12];
-signed char serialIndex = -1;
+int serialBytesToRead[SERIAL_READ_ARR_LEN];
+signed char serialIndex;
 
 /* i2c data */
 struct i2c_device_info {
@@ -102,8 +102,6 @@ byte servoCount = 0;
 
 boolean isResetting = false;
 
-int memCheckCounter = 0;
-char buffer[20];
 
 /* utility functions */
 void wireWrite(byte data)
@@ -724,11 +722,9 @@ void sysexCallback(byte command, byte argc, byte *argv)
             byte txPin, rxPin;
             serial_pins pins;
 
-            serialBytesToRead[portId] = (int)argv[4] | ((int)argv[5] << 7);
-
-            if (portId > 7 && argc > 6) {
-              rxPin = argv[6];
-              txPin = argv[7];
+            if (portId > 7 && argc > 4) {
+              rxPin = argv[4];
+              txPin = argv[5];
             }
 
             if (portId < 8) {
@@ -796,6 +792,14 @@ void sysexCallback(byte command, byte argc, byte *argv)
             if (serialIndex + 1 >= MAX_SERIAL_PORTS) {
               break;
             }
+
+            if (argc > 2) {
+              // maximum number of bytes to read from buffer per iteration of loop()
+              serialBytesToRead[portId] = (int)argv[2] | ((int)argv[3] << 7);
+            } else {
+              // read all available bytes per iteration of loop()
+              serialBytesToRead[portId] = 0;
+            }
             serialIndex++;
             reportSerial[serialIndex] = portId;
           } else if (argv[1] == SERIAL_STOP_READING) {
@@ -907,7 +911,11 @@ void systemResetCallback()
     }
   }
 #endif
+
   serialIndex = -1;
+  for (byte i = 0; i < SERIAL_READ_ARR_LEN; i++) {
+    serialBytesToRead[i] = 0;
+  }
 
   for (byte i = 0; i < TOTAL_PORTS; i++) {
     reportPINs[i] = false;    // by default, reporting off
@@ -1007,19 +1015,7 @@ void loop()
         readAndReportData(query[i].addr, query[i].reg, query[i].bytes);
       }
     }
-
-    //    // check for memory leaks
-    //    // send memory reading approximately ever 10 seconds
-    //    // 526 * 19 (default sampling interval) = 9994 ms
-    //    // increase 526 for long running processes
-    //    if (memCheckCounter++ == 526) {
-    //      sprintf(buffer, "%u", freeMemory());
-    //      Firmata.sendString(buffer);
-    //      memCheckCounter = 0;
-    //    }
-
   }
 
-  // TODO - figure out best location to call this function.
   checkSerial();
 }
diff --git a/utility/serialUtils.h b/utility/serialUtils.h
index 63255059..15cfc518 100644
--- a/utility/serialUtils.h
+++ b/utility/serialUtils.h
@@ -20,14 +20,17 @@
 #define HW_SERIAL1                  0x01
 #define HW_SERIAL2                  0x02
 #define HW_SERIAL3                  0x03
+// extensible up to 0x07
 
 #define SW_SERIAL0                  0x08
 #define SW_SERIAL1                  0x09
 #define SW_SERIAL2                  0x0A
 #define SW_SERIAL3                  0x0B
+// extensible up to 0x0F
 
 #define SERIAL_PORT_ID_MASK         0x0F
 #define MAX_SERIAL_PORTS            8
+#define SERIAL_READ_ARR_LEN         12
 
 // map configuration query response resolution value to serial pin type
 #define RES_RX1                     0x02

From c09eda92b20baf41c9879b38186bc550dfb80ed6 Mon Sep 17 00:00:00 2001
From: Jeff Hoefs <jeff.hoefs@gmail.com>
Date: Sat, 3 Oct 2015 16:49:13 -0700
Subject: [PATCH 08/10] remove unused include. Include SW serial lib when
 compiled in Arduino 1.0.x

---
 examples/StandardFirmata/StandardFirmata.ino | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/examples/StandardFirmata/StandardFirmata.ino b/examples/StandardFirmata/StandardFirmata.ino
index 7bfa2f02..a58076cc 100644
--- a/examples/StandardFirmata/StandardFirmata.ino
+++ b/examples/StandardFirmata/StandardFirmata.ino
@@ -25,11 +25,12 @@
 
 #include <Servo.h>
 #include <Wire.h>
-#include <ArduinoUnit.h>
 #include <Firmata.h>
 
 // SoftwareSerial is only supported for AVR-based boards
-#if defined(ARDUINO_ARCH_AVR)
+// The second condition checks if the IDE is in the 1.0.x series, if so, include SoftwareSerial
+// since it should be available to all boards in that IDE.
+#if defined(ARDUINO_ARCH_AVR) || (ARDUINO >= 100 && ARDUINO < 10500)
 #include <SoftwareSerial.h>
 #endif
 #include "utility/serialUtils.h"

From 4becfc5f93a19d56edd61b7d4876891b6b70cf81 Mon Sep 17 00:00:00 2001
From: Jeff Hoefs <jeff.hoefs@gmail.com>
Date: Sun, 4 Oct 2015 22:13:54 -0700
Subject: [PATCH 09/10] create new StandardFirmataPlus and
 StandardFirmataEthernetPlus

Revert StandardFirmata to 2.4.4. StandardFirmataPlus will be used
for new features. StandardFirmata will be maintained and minor
features may still be added as long as Flash, RAM and performance
of lower memory boards (Uno, Leonardo, etc) is not affected.
---
 examples/StandardFirmata/LICENSE.txt          |    0
 examples/StandardFirmata/StandardFirmata.ino  |  296 +----
 .../StandardFirmataChipKIT.ino                |    2 +-
 examples/StandardFirmataEthernet/LICENSE.txt  |  458 +++++++
 .../StandardFirmataEthernetPlus/LICENSE.txt   |  458 +++++++
 .../StandardFirmataEthernetPlus.ino           | 1131 +++++++++++++++++
 examples/StandardFirmataPlus/LICENSE.txt      |  458 +++++++
 .../StandardFirmataPlus.ino                   | 1040 +++++++++++++++
 examples/StandardFirmataYun/LICENSE.txt       |  458 +++++++
 9 files changed, 4006 insertions(+), 295 deletions(-)
 mode change 100644 => 100755 examples/StandardFirmata/LICENSE.txt
 mode change 100644 => 100755 examples/StandardFirmata/StandardFirmata.ino
 create mode 100755 examples/StandardFirmataEthernet/LICENSE.txt
 create mode 100755 examples/StandardFirmataEthernetPlus/LICENSE.txt
 create mode 100644 examples/StandardFirmataEthernetPlus/StandardFirmataEthernetPlus.ino
 create mode 100644 examples/StandardFirmataPlus/LICENSE.txt
 create mode 100644 examples/StandardFirmataPlus/StandardFirmataPlus.ino
 create mode 100755 examples/StandardFirmataYun/LICENSE.txt

diff --git a/examples/StandardFirmata/LICENSE.txt b/examples/StandardFirmata/LICENSE.txt
old mode 100644
new mode 100755
diff --git a/examples/StandardFirmata/StandardFirmata.ino b/examples/StandardFirmata/StandardFirmata.ino
old mode 100644
new mode 100755
index a58076cc..c3c88a50
--- a/examples/StandardFirmata/StandardFirmata.ino
+++ b/examples/StandardFirmata/StandardFirmata.ino
@@ -20,21 +20,13 @@
 
   See file LICENSE.txt for further informations on licensing terms.
 
-  Last updated by Jeff Hoefs: October 3rd, 2015
+  Last updated by Jeff Hoefs: August 9th, 2015
 */
 
 #include <Servo.h>
 #include <Wire.h>
 #include <Firmata.h>
 
-// SoftwareSerial is only supported for AVR-based boards
-// The second condition checks if the IDE is in the 1.0.x series, if so, include SoftwareSerial
-// since it should be available to all boards in that IDE.
-#if defined(ARDUINO_ARCH_AVR) || (ARDUINO >= 100 && ARDUINO < 10500)
-#include <SoftwareSerial.h>
-#endif
-#include "utility/serialUtils.h"
-
 #define I2C_WRITE                   B00000000
 #define I2C_READ                    B00001000
 #define I2C_READ_CONTINUOUSLY       B00010000
@@ -69,16 +61,6 @@ unsigned long currentMillis;        // store the current value from millis()
 unsigned long previousMillis;       // for comparison with currentMillis
 unsigned int samplingInterval = 19; // how often to run the main loop (in ms)
 
-/* serial message */
-Stream *swSerial0 = NULL;
-Stream *swSerial1 = NULL;
-Stream *swSerial2 = NULL;
-Stream *swSerial3 = NULL;
-
-byte reportSerial[MAX_SERIAL_PORTS];
-int serialBytesToRead[SERIAL_READ_ARR_LEN];
-signed char serialIndex;
-
 /* i2c data */
 struct i2c_device_info {
   byte addr;
@@ -103,7 +85,6 @@ byte servoCount = 0;
 
 boolean isResetting = false;
 
-
 /* utility functions */
 void wireWrite(byte data)
 {
@@ -127,103 +108,6 @@ byte wireRead(void)
  * FUNCTIONS
  *============================================================================*/
 
-// get a pointer to the serial port associated with the specified port id
-Stream* getPortFromId(byte portId)
-{
-  switch (portId) {
-    case HW_SERIAL0:
-      // block use of Serial (typically pins 0 and 1) until ability to reclaim Serial is implemented
-      //return &Serial;
-      return NULL;
-#if defined(PIN_SERIAL1_RX)
-    case HW_SERIAL1:
-      return &Serial1;
-#endif
-#if defined(PIN_SERIAL2_RX)
-    case HW_SERIAL2:
-      return &Serial2;
-#endif
-#if defined(PIN_SERIAL3_RX)
-    case HW_SERIAL3:
-      return &Serial3;
-#endif
-#if defined(SoftwareSerial_h)
-    case SW_SERIAL0:
-      if (swSerial0 != NULL) {
-        // instances of SoftwareSerial are already pointers so simply return the instance
-        return swSerial0;
-      }
-      break;
-    case SW_SERIAL1:
-      if (swSerial1 != NULL) {
-        return swSerial1;
-      }
-      break;
-    case SW_SERIAL2:
-      if (swSerial2 != NULL) {
-        return swSerial2;
-      }
-      break;
-    case SW_SERIAL3:
-      if (swSerial3 != NULL) {
-        return swSerial3;
-      }
-      break;
-#endif
-  }
-  return NULL;
-}
-
-// Check serial ports that have READ_CONTINUOUS mode set and relay any data
-// for each port to the device attached to that port.
-void checkSerial()
-{
-  byte portId, serialData;
-  int bytesToRead = 0;
-  int numBytesToRead = 0;
-  Stream* serialPort;
-
-  if (serialIndex > -1) {
-
-    // loop through all reporting (READ_CONTINUOUS) serial ports
-    for (byte i = 0; i < serialIndex + 1; i++) {
-      portId = reportSerial[i];
-      bytesToRead = serialBytesToRead[portId];
-      serialPort = getPortFromId(portId);
-      if (serialPort == NULL) {
-        continue;
-      }
-#if defined(SoftwareSerial_h)
-      // only the SoftwareSerial port that is "listening" can read data
-      if (portId > 7 && !((SoftwareSerial*)serialPort)->isListening()) {
-        continue;
-      }
-#endif
-      if (serialPort->available() > 0) {
-        Firmata.write(START_SYSEX);
-        Firmata.write(SERIAL_MESSAGE);
-        Firmata.write(SERIAL_REPLY | portId);
-
-        if (bytesToRead == 0 || (serialPort->available() <= bytesToRead)) {
-          numBytesToRead = serialPort->available();
-        } else {
-          numBytesToRead = bytesToRead;
-        }
-
-        // relay serial data to the serial device
-        while (numBytesToRead > 0) {
-          serialData = serialPort->read();
-          Firmata.write(serialData & 0x7F);
-          Firmata.write((serialData >> 7) & 0x7F);
-          numBytesToRead--;
-        }
-        Firmata.write(END_SYSEX);
-      }
-
-    }
-  }
-}
-
 void attachServo(byte pin, int minPulse, int maxPulse)
 {
   if (servoCount < MAX_SERVOS) {
@@ -414,10 +298,6 @@ void setPinModeCallback(byte pin, int mode)
         pinConfig[pin] = I2C;
       }
       break;
-    case MODE_SERIAL:
-      // used for both HW and SW serial
-      pinConfig[pin] = MODE_SERIAL;
-      break;
     default:
       Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
   }
@@ -679,10 +559,6 @@ void sysexCallback(byte command, byte argc, byte *argv)
           Firmata.write(I2C);
           Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
         }
-        if (IS_PIN_SERIAL(pin)) {
-          Firmata.write(MODE_SERIAL);
-          Firmata.write(getSerialPinType(pin));
-        }
         Firmata.write(127);
       }
       Firmata.write(END_SYSEX);
@@ -710,155 +586,6 @@ void sysexCallback(byte command, byte argc, byte *argv)
       }
       Firmata.write(END_SYSEX);
       break;
-
-    case SERIAL_MESSAGE:
-      Stream * serialPort;
-      mode = argv[0] & SERIAL_MODE_MASK;
-      byte portId = argv[0] & SERIAL_PORT_ID_MASK;
-
-      switch (mode) {
-        case SERIAL_CONFIG:
-          {
-            long baud = (long)argv[1] | ((long)argv[2] << 7) | ((long)argv[3] << 14);
-            byte txPin, rxPin;
-            serial_pins pins;
-
-            if (portId > 7 && argc > 4) {
-              rxPin = argv[4];
-              txPin = argv[5];
-            }
-
-            if (portId < 8) {
-              serialPort = getPortFromId(portId);
-              if (serialPort != NULL) {
-                pins = getSerialPinNumbers(portId);
-                if (pins.rx != 0 && pins.tx != 0) {
-                  setPinModeCallback(pins.rx, MODE_SERIAL);
-                  setPinModeCallback(pins.tx, MODE_SERIAL);
-                  // Fixes an issue where some serial devices would not work properly with Arduino Due
-                  // because all Arduino pins are set to OUTPUT by default in StandardFirmata.
-                  pinMode(pins.rx, INPUT);
-                }
-                ((HardwareSerial*)serialPort)->begin(baud);
-              }
-            } else {
-#if defined(SoftwareSerial_h)
-              switch (portId) {
-                case SW_SERIAL0:
-                  if (swSerial0 == NULL) {
-                    swSerial0 = new SoftwareSerial(rxPin, txPin);
-                  }
-                  break;
-                case SW_SERIAL1:
-                  if (swSerial1 == NULL) {
-                    swSerial1 = new SoftwareSerial(rxPin, txPin);
-                  }
-                  break;
-                case SW_SERIAL2:
-                  if (swSerial2 == NULL) {
-                    swSerial2 = new SoftwareSerial(rxPin, txPin);
-                  }
-                  break;
-                case SW_SERIAL3:
-                  if (swSerial3 == NULL) {
-                    swSerial3 = new SoftwareSerial(rxPin, txPin);
-                  }
-                  break;
-              }
-              serialPort = getPortFromId(portId);
-              if (serialPort != NULL) {
-                setPinModeCallback(rxPin, MODE_SERIAL);
-                setPinModeCallback(txPin, MODE_SERIAL);
-                ((SoftwareSerial*)serialPort)->begin(baud);
-              }
-#endif
-            }
-            break; // SERIAL_CONFIG
-          }
-        case SERIAL_WRITE:
-          {
-            byte data;
-            serialPort = getPortFromId(portId);
-            if (serialPort == NULL) {
-              break;
-            }
-            for (byte i = 1; i < argc; i += 2) {
-              data = argv[i] + (argv[i + 1] << 7);
-              serialPort->write(data);
-            }
-            break; // SERIAL_WRITE
-          }
-        case SERIAL_READ:
-          if (argv[1] == SERIAL_READ_CONTINUOUSLY) {
-            if (serialIndex + 1 >= MAX_SERIAL_PORTS) {
-              break;
-            }
-
-            if (argc > 2) {
-              // maximum number of bytes to read from buffer per iteration of loop()
-              serialBytesToRead[portId] = (int)argv[2] | ((int)argv[3] << 7);
-            } else {
-              // read all available bytes per iteration of loop()
-              serialBytesToRead[portId] = 0;
-            }
-            serialIndex++;
-            reportSerial[serialIndex] = portId;
-          } else if (argv[1] == SERIAL_STOP_READING) {
-            byte serialIndexToSkip;
-            if (serialIndex <= 0) {
-              serialIndex = -1;
-            } else {
-              for (byte i = 0; i < serialIndex + 1; i++) {
-                if (reportSerial[i] == portId) {
-                  serialIndexToSkip = i;
-                  break;
-                }
-              }
-              // shift elements over to fill space left by removed element
-              for (byte i = serialIndexToSkip; i < serialIndex + 1; i++) {
-                if (i < MAX_SERIAL_PORTS) {
-                  reportSerial[i] = reportSerial[i + 1];
-                }
-              }
-              serialIndex--;
-            }
-          }
-          break; // SERIAL_READ
-        case SERIAL_CLOSE:
-          serialPort = getPortFromId(portId);
-          if (serialPort != NULL) {
-            if (portId < 8) {
-              ((HardwareSerial*)serialPort)->end();
-            } else {
-#if defined(SoftwareSerial_h)
-              ((SoftwareSerial*)serialPort)->end();
-              if (serialPort != NULL) {
-                free(serialPort);
-                serialPort = NULL;
-              }
-#endif
-            }
-          }
-          break; // SERIAL_CLOSE
-        case SERIAL_FLUSH:
-          serialPort = getPortFromId(portId);
-          if (serialPort != NULL) {
-            getPortFromId(portId)->flush();
-          }
-          break; // SERIAL_FLUSH
-#if defined(SoftwareSerial_h)
-        case SERIAL_LISTEN:
-          // can only call listen() on software serial ports
-          if (portId > 7) {
-            serialPort = getPortFromId(portId);
-            if (serialPort != NULL) {
-              ((SoftwareSerial*)serialPort)->listen();
-            }
-          }
-          break; // SERIAL_LISTEN
-#endif
-      }
-      break;
   }
 }
 
@@ -892,7 +619,6 @@ void disableI2CPins() {
 
 void systemResetCallback()
 {
-  Stream *serialPort;
   isResetting = true;
 
   // initialize a defalt state
@@ -902,22 +628,6 @@ void systemResetCallback()
     disableI2CPins();
   }
 
-#if defined(SoftwareSerial_h)
-  // free memory allocated for SoftwareSerial ports
-  for (byte i = SW_SERIAL0; i < SW_SERIAL3 + 1; i++) {
-    serialPort = getPortFromId(i);
-    if (serialPort != NULL) {
-      free(serialPort);
-      serialPort = NULL;
-    }
-  }
-#endif
-
-  serialIndex = -1;
-  for (byte i = 0; i < SERIAL_READ_ARR_LEN; i++) {
-    serialBytesToRead[i] = 0;
-  }
-
   for (byte i = 0; i < TOTAL_PORTS; i++) {
     reportPINs[i] = false;    // by default, reporting off
     portConfigInputs[i] = 0;  // until activated
@@ -971,7 +681,7 @@ void setup()
   // Call begin(baud) on the alternate serial port and pass it to Firmata to begin like this:
   // Serial1.begin(57600);
   // Firmata.begin(Serial1);
-  // However do not do this if you are using SERIAL_MESSAGE
+  // then comment out or remove lines 701 - 704 below
 
   Firmata.begin(57600);
   while (!Serial) {
@@ -1017,6 +727,4 @@ void loop()
       }
     }
   }
-
-  checkSerial();
 }
diff --git a/examples/StandardFirmataChipKIT/StandardFirmataChipKIT.ino b/examples/StandardFirmataChipKIT/StandardFirmataChipKIT.ino
index dd2d8684..4385ef96 100644
--- a/examples/StandardFirmataChipKIT/StandardFirmataChipKIT.ino
+++ b/examples/StandardFirmataChipKIT/StandardFirmataChipKIT.ino
@@ -38,7 +38,7 @@
 #define I2C_REGISTER_NOT_SPECIFIED  -1
 
 // the minimum interval for sampling analog input
-#define MINIMUM_SAMPLING_INTERVAL 10
+#define MINIMUM_SAMPLING_INTERVAL   10
 
 
 /*==============================================================================
diff --git a/examples/StandardFirmataEthernet/LICENSE.txt b/examples/StandardFirmataEthernet/LICENSE.txt
new file mode 100755
index 00000000..77cec6dd
--- /dev/null
+++ b/examples/StandardFirmataEthernet/LICENSE.txt
@@ -0,0 +1,458 @@
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
diff --git a/examples/StandardFirmataEthernetPlus/LICENSE.txt b/examples/StandardFirmataEthernetPlus/LICENSE.txt
new file mode 100755
index 00000000..77cec6dd
--- /dev/null
+++ b/examples/StandardFirmataEthernetPlus/LICENSE.txt
@@ -0,0 +1,458 @@
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
diff --git a/examples/StandardFirmataEthernetPlus/StandardFirmataEthernetPlus.ino b/examples/StandardFirmataEthernetPlus/StandardFirmataEthernetPlus.ino
new file mode 100644
index 00000000..1062249c
--- /dev/null
+++ b/examples/StandardFirmataEthernetPlus/StandardFirmataEthernetPlus.ino
@@ -0,0 +1,1131 @@
+/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please clink on the following link
+  to open the list of Firmata client libraries your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2009-2015 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated by Jeff Hoefs: October 4th, 2015
+*/
+
+/*
+  README
+
+  StandardFirmataEthernetPlus adds additional features that may exceed the Flash and
+  RAM sizes of Arduino boards such as ATMega328p (Uno) and ATMega32u4
+  (Leonardo, Micro, Yun, etc). It is best to use StandardFirmataPlus with higher
+  memory boards such as the Arduino Mega, Arduino Due, Teensy 3.0/3.1/3.2, etc.
+
+  This sketch consumes too much Flash and RAM to run reliably on an
+  Arduino Ethernet board, Arduino Leonardo, Yun, Uno or other ATMega328p or
+  ATMega32u4-based boards. You will need a board with > 32k Flash and 3k RAM.
+  Use StandardFirmataEthernet.ino instead for those boards and other boards that do not
+  meet the Flash and RAM requirements.
+
+  USE: To use StandardFirmataEthernetPlus you will need to have an Arduino Ethernet shield
+  (or clone).
+
+  Follow the instructions in the NETWORK CONFIGURATION section below to
+  configure your particular hardware.
+
+  NOTE: If you are using an Arduino Ethernet shield you cannot use the following pins on
+  the following boards. Firmata will ignore any requests to use these pins:
+
+  - Arduino Mega: (D4, D10, D50, D51, D52, D53)
+  - Arduino Due: (D4, D10)
+*/
+
+#include <Servo.h>
+#include <Wire.h>
+#include <Firmata.h>
+
+// SoftwareSerial is only supported for AVR-based boards
+// The second condition checks if the IDE is in the 1.0.x series, if so, include SoftwareSerial
+// since it should be available to all boards in that IDE.
+#if defined(ARDUINO_ARCH_AVR) || (ARDUINO >= 100 && ARDUINO < 10500)
+#include <SoftwareSerial.h>
+#endif
+#include "utility/serialUtils.h"
+
+//#define SERIAL_DEBUG
+#include "utility/firmataDebug.h"
+
+#define I2C_WRITE                   B00000000
+#define I2C_READ                    B00001000
+#define I2C_READ_CONTINUOUSLY       B00010000
+#define I2C_STOP_READING            B00011000
+#define I2C_READ_WRITE_MODE_MASK    B00011000
+#define I2C_10BIT_ADDRESS_MODE_MASK B00100000
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   10
+
+
+/*==============================================================================
+ * NETWORK CONFIGURATION
+ *
+ * You must configure your particular hardware. Follow the steps below.
+ *============================================================================*/
+
+#include <SPI.h>
+#include <Ethernet.h>
+
+// STEP 1 [REQUIRED for all boards and shields]
+// replace with IP of the server you want to connect to, comment out if using 'remote_host'
+#define remote_ip IPAddress(10, 0, 0, 3)
+// *** REMOTE HOST IS NOT YET WORKING ***
+// replace with hostname of server you want to connect to, comment out if using 'remote_ip'
+// #define remote_host "server.local"
+
+// STEP 2 [REQUIRED]
+// Replace with the port that your server is listening on
+#define remote_port 3030
+
+// STEP 3 [REQUIRED if not using DHCP]
+// Replace with your board or ethernet shield's IP address
+// Comment out if you want to use DHCP
+#define local_ip IPAddress(10, 0, 0, 15)
+
+// STEP 4 [REQUIRED]
+// replace with ethernet shield mac. Must be unique for your network
+const byte mac[] = {0x90, 0xA2, 0xDA, 0x00, 0x53, 0xE5};
+
+#if defined remote_ip && defined remote_host
+#error "cannot define both remote_ip and remote_host at the same time!"
+#endif
+
+
+/*==============================================================================
+ * GLOBAL VARIABLES
+ *============================================================================*/
+
+/* network */
+
+#include "utility/EthernetClientStream.h"
+
+EthernetClient client;
+
+#if defined remote_ip && !defined remote_host
+#ifdef local_ip
+EthernetClientStream stream(client, local_ip, remote_ip, NULL, remote_port);
+#else
+EthernetClientStream stream(client, IPAddress(0, 0, 0, 0), remote_ip, NULL, remote_port);
+#endif
+#endif
+
+#if !defined remote_ip && defined remote_host
+#ifdef local_ip
+EthernetClientStream stream(client, local_ip, IPAddress(0, 0, 0, 0), remote_host, remote_port);
+#else
+EthernetClientStream stream(client, IPAddress(0, 0, 0, 0), IPAddress(0, 0, 0, 0), remote_host, remote_port);
+#endif
+#endif
+
+/* analog inputs */
+int analogInputsToReport = 0;      // bitwise array to store pin reporting
+
+/* digital input ports */
+byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
+byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
+
+/* pins configuration */
+byte pinConfig[TOTAL_PINS];         // configuration of every pin
+byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
+int pinState[TOTAL_PINS];           // any value that has been written
+
+/* timer variables */
+unsigned long currentMillis;        // store the current value from millis()
+unsigned long previousMillis;       // for comparison with currentMillis
+unsigned int samplingInterval = 19; // how often to sample analog inputs (in ms)
+
+/* serial message */
+Stream *swSerial0 = NULL;
+Stream *swSerial1 = NULL;
+Stream *swSerial2 = NULL;
+Stream *swSerial3 = NULL;
+
+byte reportSerial[MAX_SERIAL_PORTS];
+int serialBytesToRead[SERIAL_READ_ARR_LEN];
+signed char serialIndex;
+
+/* i2c data */
+struct i2c_device_info {
+  byte addr;
+  int reg;
+  byte bytes;
+};
+
+/* for i2c read continuous mode */
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[32];
+boolean isI2CEnabled = false;
+signed char queryIndex = -1;
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
+
+Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
+/*==============================================================================
+ * FUNCTIONS
+ *============================================================================*/
+
+// get a pointer to the serial port associated with the specified port id
+Stream* getPortFromId(byte portId)
+{
+  switch (portId) {
+    case HW_SERIAL0:
+      // block use of Serial (typically pins 0 and 1) until ability to reclaim Serial is implemented
+      //return &Serial;
+      return NULL;
+#if defined(PIN_SERIAL1_RX)
+    case HW_SERIAL1:
+      return &Serial1;
+#endif
+#if defined(PIN_SERIAL2_RX)
+    case HW_SERIAL2:
+      return &Serial2;
+#endif
+#if defined(PIN_SERIAL3_RX)
+    case HW_SERIAL3:
+      return &Serial3;
+#endif
+#if defined(SoftwareSerial_h)
+    case SW_SERIAL0:
+      if (swSerial0 != NULL) {
+        // instances of SoftwareSerial are already pointers so simply return the instance
+        return swSerial0;
+      }
+      break;
+    case SW_SERIAL1:
+      if (swSerial1 != NULL) {
+        return swSerial1;
+      }
+      break;
+    case SW_SERIAL2:
+      if (swSerial2 != NULL) {
+        return swSerial2;
+      }
+      break;
+    case SW_SERIAL3:
+      if (swSerial3 != NULL) {
+        return swSerial3;
+      }
+      break;
+#endif
+  }
+  return NULL;
+}
+
+// Check serial ports that have READ_CONTINUOUS mode set and relay any data
+// for each port to the device attached to that port.
+void checkSerial()
+{
+  byte portId, serialData;
+  int bytesToRead = 0;
+  int numBytesToRead = 0;
+  Stream* serialPort;
+
+  if (serialIndex > -1) {
+
+    // loop through all reporting (READ_CONTINUOUS) serial ports
+    for (byte i = 0; i < serialIndex + 1; i++) {
+      portId = reportSerial[i];
+      bytesToRead = serialBytesToRead[portId];
+      serialPort = getPortFromId(portId);
+      if (serialPort == NULL) {
+        continue;
+      }
+#if defined(SoftwareSerial_h)
+      // only the SoftwareSerial port that is "listening" can read data
+      if (portId > 7 && !((SoftwareSerial*)serialPort)->isListening()) {
+        continue;
+      }
+#endif
+      if (serialPort->available() > 0) {
+        Firmata.write(START_SYSEX);
+        Firmata.write(SERIAL_MESSAGE);
+        Firmata.write(SERIAL_REPLY | portId);
+
+        if (bytesToRead == 0 || (serialPort->available() <= bytesToRead)) {
+          numBytesToRead = serialPort->available();
+        } else {
+          numBytesToRead = bytesToRead;
+        }
+
+        // relay serial data to the serial device
+        while (numBytesToRead > 0) {
+          serialData = serialPort->read();
+          Firmata.write(serialData & 0x7F);
+          Firmata.write((serialData >> 7) & 0x7F);
+          numBytesToRead--;
+        }
+        Firmata.write(END_SYSEX);
+      }
+
+    }
+  }
+}
+
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes) {
+  // allow I2C requests that don't require a register read
+  // for example, some devices using an interrupt pin to signify new data available
+  // do not always require the register read so upon interrupt you call Wire.requestFrom()
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
+    Wire.beginTransmission(address);
+    wireWrite((byte)theRegister);
+    Wire.endTransmission();
+    // do not set a value of 0
+    if (i2cReadDelayTime > 0) {
+      // delay is necessary for some devices such as WiiNunchuck
+      delayMicroseconds(i2cReadDelayTime);
+    }
+  } else {
+    theRegister = 0;  // fill the register with a dummy value
+  }
+
+  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
+
+  // check to be sure correct number of bytes were returned by slave
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
+  }
+
+  // send slave address, register and received bytes
+  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
+}
+
+void outputPort(byte portNumber, byte portValue, byte forceSend)
+{
+  // pins not configured as INPUT are cleared to zeros
+  portValue = portValue & portConfigInputs[portNumber];
+  // only send if the value is different than previously sent
+  if (forceSend || previousPINs[portNumber] != portValue) {
+    Firmata.sendDigitalPort(portNumber, portValue);
+    previousPINs[portNumber] = portValue;
+  }
+}
+
+/* -----------------------------------------------------------------------------
+ * check all the active digital inputs for change of state, then add any events
+ * to the Stream output queue using Stream.write() */
+void checkDigitalInputs(void)
+{
+  /* Using non-looping code allows constants to be given to readPort().
+   * The compiler will apply substantial optimizations if the inputs
+   * to readPort() are compile-time constants. */
+  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
+  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
+  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
+  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
+  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
+  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
+  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
+  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
+  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
+  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
+  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
+  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
+  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
+  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
+  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
+  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
+}
+
+// -----------------------------------------------------------------------------
+/* sets the pin mode to the correct state and sets the relevant bits in the
+ * two bit-arrays that track Digital I/O and PWM status
+ */
+void setPinModeCallback(byte pin, int mode)
+{
+  if (pinConfig[pin] == IGNORE)
+    return;
+
+  if (pinConfig[pin] == I2C && isI2CEnabled && mode != I2C) {
+    // disable i2c so pins can be used for other functions
+    // the following if statements should reconfigure the pins properly
+    disableI2CPins();
+  }
+  if (IS_PIN_DIGITAL(pin) && mode != SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
+  }
+  if (IS_PIN_ANALOG(pin)) {
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == ANALOG ? 1 : 0); // turn on/off reporting
+  }
+  if (IS_PIN_DIGITAL(pin)) {
+    if (mode == INPUT) {
+      portConfigInputs[pin / 8] |= (1 << (pin & 7));
+    } else {
+      portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+    }
+  }
+  pinState[pin] = 0;
+  switch (mode) {
+    case ANALOG:
+      if (IS_PIN_ANALOG(pin)) {
+        if (IS_PIN_DIGITAL(pin)) {
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+        }
+        pinConfig[pin] = ANALOG;
+      }
+      break;
+    case INPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+        pinConfig[pin] = INPUT;
+      }
+      break;
+    case OUTPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
+        pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+        pinConfig[pin] = OUTPUT;
+      }
+      break;
+    case PWM:
+      if (IS_PIN_PWM(pin)) {
+        pinMode(PIN_TO_PWM(pin), OUTPUT);
+        analogWrite(PIN_TO_PWM(pin), 0);
+        pinConfig[pin] = PWM;
+      }
+      break;
+    case SERVO:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinConfig[pin] = SERVO;
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case I2C:
+      if (IS_PIN_I2C(pin)) {
+        // mark the pin as i2c
+        // the user must call I2C_CONFIG to enable I2C for a device
+        pinConfig[pin] = I2C;
+      }
+      break;
+    default:
+      Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+void analogWriteCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS) {
+    switch (pinConfig[pin]) {
+      case SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        pinState[pin] = value;
+        break;
+      case PWM:
+        if (IS_PIN_PWM(pin))
+          analogWrite(PIN_TO_PWM(pin), value);
+        pinState[pin] = value;
+        break;
+    }
+  }
+}
+
+void digitalWriteCallback(byte port, int value)
+{
+  byte pin, lastPin, mask = 1, pinWriteMask = 0;
+
+  if (port < TOTAL_PORTS) {
+    // create a mask of the pins on this port that are writable.
+    lastPin = port * 8 + 8;
+    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
+    for (pin = port * 8; pin < lastPin; pin++) {
+      // do not disturb non-digital pins (eg, Rx & Tx)
+      if (IS_PIN_DIGITAL(pin)) {
+        // only write to OUTPUT and INPUT (enables pullup)
+        // do not touch pins in PWM, ANALOG, SERVO or other modes
+        if (pinConfig[pin] == OUTPUT || pinConfig[pin] == INPUT) {
+          pinWriteMask |= mask;
+          pinState[pin] = ((byte)value & mask) ? 1 : 0;
+        }
+      }
+      mask = mask << 1;
+    }
+    writePort(port, (byte)value, pinWriteMask);
+  }
+}
+
+
+// -----------------------------------------------------------------------------
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+void reportDigitalCallback(byte port, int value)
+{
+  if (port < TOTAL_PORTS) {
+    reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
+  }
+  // do not disable analog reporting on these 8 pins, to allow some
+  // pins used for digital, others analog.  Instead, allow both types
+  // of reporting to be enabled, but check if the pin is configured
+  // as analog when sampling the analog inputs.  Likewise, while
+  // scanning digital pins, portConfigInputs will mask off values from any
+  // pins configured as analog
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  byte mode;
+  byte slaveAddress;
+  byte data;
+  int slaveRegister;
+  unsigned int delayTime;
+
+  switch (command) {
+    case I2C_REQUEST:
+      mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
+      if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
+        Firmata.sendString("10-bit addressing not supported");
+        return;
+      }
+      else {
+        slaveAddress = argv[0];
+      }
+
+      switch (mode) {
+        case I2C_WRITE:
+          Wire.beginTransmission(slaveAddress);
+          for (byte i = 2; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            wireWrite(data);
+          }
+          Wire.endTransmission();
+          delayMicroseconds(70);
+          break;
+        case I2C_READ:
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          readAndReportData(slaveAddress, (int)slaveRegister, data);
+          break;
+        case I2C_READ_CONTINUOUSLY:
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
+            // too many queries, just ignore
+            Firmata.sendString("too many I2C queries");
+            break;
+          }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          queryIndex++;
+          query[queryIndex].addr = slaveAddress;
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          break;
+        case I2C_STOP_READING:
+          byte queryIndexToSkip;
+          // if read continuous mode is enabled for only 1 i2c device, disable
+          // read continuous reporting for that device
+          if (queryIndex <= 0) {
+            queryIndex = -1;
+          } else {
+            // if read continuous mode is enabled for multiple devices,
+            // determine which device to stop reading and remove it's data from
+            // the array, shifiting other array data to fill the space
+            for (byte i = 0; i < queryIndex + 1; i++) {
+              if (query[i].addr == slaveAddress) {
+                queryIndexToSkip = i;
+                break;
+              }
+            }
+
+            for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
+              if (i < I2C_MAX_QUERIES) {
+                query[i].addr = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
+                query[i].bytes = query[i + 1].bytes;
+              }
+            }
+            queryIndex--;
+          }
+          break;
+        default:
+          break;
+      }
+      break;
+    case I2C_CONFIG:
+      delayTime = (argv[0] + (argv[1] << 7));
+
+      if (delayTime > 0) {
+        i2cReadDelayTime = delayTime;
+      }
+
+      if (!isI2CEnabled) {
+        enableI2CPins();
+      }
+
+      break;
+    case SERVO_CONFIG:
+      if (argc > 4) {
+        // these vars are here for clarity, they'll optimized away by the compiler
+        byte pin = argv[0];
+        int minPulse = argv[1] + (argv[2] << 7);
+        int maxPulse = argv[3] + (argv[4] << 7);
+
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, SERVO);
+        }
+      }
+      break;
+    case SAMPLING_INTERVAL:
+      if (argc > 1) {
+        samplingInterval = argv[0] + (argv[1] << 7);
+        if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+          samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+        }
+      } else {
+        //Firmata.sendString("Not enough data");
+      }
+      break;
+    case EXTENDED_ANALOG:
+      if (argc > 1) {
+        int val = argv[1];
+        if (argc > 2) val |= (argv[2] << 7);
+        if (argc > 3) val |= (argv[3] << 14);
+        analogWriteCallback(argv[0], val);
+      }
+      break;
+    case CAPABILITY_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(CAPABILITY_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write((byte)INPUT);
+          Firmata.write(1);
+          Firmata.write((byte)OUTPUT);
+          Firmata.write(1);
+        }
+        if (IS_PIN_ANALOG(pin)) {
+          Firmata.write(ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
+        }
+        if (IS_PIN_PWM(pin)) {
+          Firmata.write(PWM);
+          Firmata.write(8); // 8 = 8-bit resolution
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(SERVO);
+          Firmata.write(14);
+        }
+        if (IS_PIN_I2C(pin)) {
+          Firmata.write(I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+        Firmata.write(127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+    case PIN_STATE_QUERY:
+      if (argc > 0) {
+        byte pin = argv[0];
+        Firmata.write(START_SYSEX);
+        Firmata.write(PIN_STATE_RESPONSE);
+        Firmata.write(pin);
+        if (pin < TOTAL_PINS) {
+          Firmata.write((byte)pinConfig[pin]);
+          Firmata.write((byte)pinState[pin] & 0x7F);
+          if (pinState[pin] & 0xFF80) Firmata.write((byte)(pinState[pin] >> 7) & 0x7F);
+          if (pinState[pin] & 0xC000) Firmata.write((byte)(pinState[pin] >> 14) & 0x7F);
+        }
+        Firmata.write(END_SYSEX);
+      }
+      break;
+    case ANALOG_MAPPING_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(ANALOG_MAPPING_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        Firmata.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+
+    case SERIAL_MESSAGE:
+      Stream * serialPort;
+      mode = argv[0] & SERIAL_MODE_MASK;
+      byte portId = argv[0] & SERIAL_PORT_ID_MASK;
+
+      switch (mode) {
+        case SERIAL_CONFIG:
+          {
+            long baud = (long)argv[1] | ((long)argv[2] << 7) | ((long)argv[3] << 14);
+            byte txPin, rxPin;
+            serial_pins pins;
+
+            if (portId > 7 && argc > 4) {
+              rxPin = argv[4];
+              txPin = argv[5];
+            }
+
+            if (portId < 8) {
+              serialPort = getPortFromId(portId);
+              if (serialPort != NULL) {
+                pins = getSerialPinNumbers(portId);
+                if (pins.rx != 0 && pins.tx != 0) {
+                  setPinModeCallback(pins.rx, MODE_SERIAL);
+                  setPinModeCallback(pins.tx, MODE_SERIAL);
+                  // Fixes an issue where some serial devices would not work properly with Arduino Due
+                  // because all Arduino pins are set to OUTPUT by default in StandardFirmata.
+                  pinMode(pins.rx, INPUT);
+                }
+                ((HardwareSerial*)serialPort)->begin(baud);
+              }
+            } else {
+#if defined(SoftwareSerial_h)
+              switch (portId) {
+                case SW_SERIAL0:
+                  if (swSerial0 == NULL) {
+                    swSerial0 = new SoftwareSerial(rxPin, txPin);
+                  }
+                  break;
+                case SW_SERIAL1:
+                  if (swSerial1 == NULL) {
+                    swSerial1 = new SoftwareSerial(rxPin, txPin);
+                  }
+                  break;
+                case SW_SERIAL2:
+                  if (swSerial2 == NULL) {
+                    swSerial2 = new SoftwareSerial(rxPin, txPin);
+                  }
+                  break;
+                case SW_SERIAL3:
+                  if (swSerial3 == NULL) {
+                    swSerial3 = new SoftwareSerial(rxPin, txPin);
+                  }
+                  break;
+              }
+              serialPort = getPortFromId(portId);
+              if (serialPort != NULL) {
+                setPinModeCallback(rxPin, MODE_SERIAL);
+                setPinModeCallback(txPin, MODE_SERIAL);
+                ((SoftwareSerial*)serialPort)->begin(baud);
+              }
+#endif
+            }
+            break; // SERIAL_CONFIG
+          }
+        case SERIAL_WRITE:
+          {
+            byte data;
+            serialPort = getPortFromId(portId);
+            if (serialPort == NULL) {
+              break;
+            }
+            for (byte i = 1; i < argc; i += 2) {
+              data = argv[i] + (argv[i + 1] << 7);
+              serialPort->write(data);
+            }
+            break; // SERIAL_WRITE
+          }
+        case SERIAL_READ:
+          if (argv[1] == SERIAL_READ_CONTINUOUSLY) {
+            if (serialIndex + 1 >= MAX_SERIAL_PORTS) {
+              break;
+            }
+
+            if (argc > 2) {
+              // maximum number of bytes to read from buffer per iteration of loop()
+              serialBytesToRead[portId] = (int)argv[2] | ((int)argv[3] << 7);
+            } else {
+              // read all available bytes per iteration of loop()
+              serialBytesToRead[portId] = 0;
+            }
+            serialIndex++;
+            reportSerial[serialIndex] = portId;
+          } else if (argv[1] == SERIAL_STOP_READING) {
+            byte serialIndexToSkip;
+            if (serialIndex <= 0) {
+              serialIndex = -1;
+            } else {
+              for (byte i = 0; i < serialIndex + 1; i++) {
+                if (reportSerial[i] == portId) {
+                  serialIndexToSkip = i;
+                  break;
+                }
+              }
+              // shift elements over to fill space left by removed element
+              for (byte i = serialIndexToSkip; i < serialIndex + 1; i++) {
+                if (i < MAX_SERIAL_PORTS) {
+                  reportSerial[i] = reportSerial[i + 1];
+                }
+              }
+              serialIndex--;
+            }
+          }
+          break; // SERIAL_READ
+        case SERIAL_CLOSE:
+          serialPort = getPortFromId(portId);
+          if (serialPort != NULL) {
+            if (portId < 8) {
+              ((HardwareSerial*)serialPort)->end();
+            } else {
+#if defined(SoftwareSerial_h)
+              ((SoftwareSerial*)serialPort)->end();
+              if (serialPort != NULL) {
+                free(serialPort);
+                serialPort = NULL;
+              }
+#endif
+            }
+          }
+          break; // SERIAL_CLOSE
+        case SERIAL_FLUSH:
+          serialPort = getPortFromId(portId);
+          if (serialPort != NULL) {
+            getPortFromId(portId)->flush();
+          }
+          break; // SERIAL_FLUSH
+#if defined(SoftwareSerial_h)
+        case SERIAL_LISTEN:
+          // can only call listen() on software serial ports
+          if (portId > 7) {
+            serialPort = getPortFromId(portId);
+            if (serialPort != NULL) {
+              ((SoftwareSerial*)serialPort)->listen();
+            }
+          }
+          break; // SERIAL_LISTEN
+#endif
+      }
+      break;
+  }
+}
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+/*==============================================================================
+ * SETUP()
+ *============================================================================*/
+
+void systemResetCallback()
+{
+  Stream *serialPort;
+  isResetting = true;
+
+  // initialize a defalt state
+  // TODO: option to load config from EEPROM instead of default
+
+  if (isI2CEnabled) {
+    disableI2CPins();
+  }
+
+#if defined(SoftwareSerial_h)
+  // free memory allocated for SoftwareSerial ports
+  for (byte i = SW_SERIAL0; i < SW_SERIAL3 + 1; i++) {
+    serialPort = getPortFromId(i);
+    if (serialPort != NULL) {
+      free(serialPort);
+      serialPort = NULL;
+    }
+  }
+#endif
+
+  serialIndex = -1;
+  for (byte i = 0; i < SERIAL_READ_ARR_LEN; i++) {
+    serialBytesToRead[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PORTS; i++) {
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
+    previousPINs[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
+    if (IS_PIN_ANALOG(i)) {
+      // turns off pullup, configures everything
+      setPinModeCallback(i, ANALOG);
+    } else {
+      // sets the output to 0, configures portConfigInputs
+      setPinModeCallback(i, OUTPUT);
+    }
+
+    servoPinMap[i] = 255;
+  }
+  // by default, do not report any analog inputs
+  analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
+
+  /* send digital inputs to set the initial state on the host computer,
+   * since once in the loop(), this firmware will only send on change */
+  /*
+  TODO: this can never execute, since no pins default to digital input
+        but it will be needed when/if we support EEPROM stored config
+  for (byte i=0; i < TOTAL_PORTS; i++) {
+    outputPort(i, readPort(i, portConfigInputs[i]), true);
+  }
+  */
+  isResetting = false;
+}
+
+void setup()
+{
+  DEBUG_BEGIN(9600);
+
+#ifdef local_ip
+  Ethernet.begin((uint8_t *)mac, local_ip); //start ethernet
+#else
+  Ethernet.begin((uint8_t *)mac);           //start ethernet using dhcp
+#endif
+
+  DEBUG_PRINTLN("connecting...");
+
+  Firmata.setFirmwareVersion(FIRMATA_MAJOR_VERSION, FIRMATA_MINOR_VERSION);
+
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
+  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
+  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
+  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(START_SYSEX, sysexCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
+
+  // StandardFirmataEthernet communicates with Ethernet shields over SPI. Therefor all
+  // SPI pins must be set to IGNORE. Otherwise Firmata would break SPI communication.
+  // add Pin 10 and configure pin 53 as output if using a MEGA with an Ethernet shield.
+
+  // ignore SPI and pin 4 that is SS for SD-Card on Ethernet-shield
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_SPI(i)
+        || 4 == i  // SD-Card on Ethernet-shiedl uses pin 4 for SS
+        || 10 == i // Ethernet-shield uses pin 10 for SS
+       ) {
+      pinConfig[i] = IGNORE;
+    }
+  }
+
+  // Arduino EthernetShield has SD SS wired to D4
+  pinMode(PIN_TO_DIGITAL(4), OUTPUT);    // switch off SD card bypassing Firmata
+  digitalWrite(PIN_TO_DIGITAL(4), HIGH); // SS is active low;
+
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+  pinMode(PIN_TO_DIGITAL(53), OUTPUT); // configure hardware SS as output on MEGA
+#endif
+
+  // start up Network Firmata:
+  Firmata.begin(stream);
+  systemResetCallback();  // reset to default config
+}
+
+/*==============================================================================
+ * LOOP()
+ *============================================================================*/
+void loop()
+{
+  byte pin, analogPin;
+
+  /* DIGITALREAD - as fast as possible, check for changes and output them to the
+   * Stream buffer using Stream.write()  */
+  checkDigitalInputs();
+
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
+   * checking digital inputs.  */
+  while (Firmata.available())
+    Firmata.processInput();
+
+  // TODO - ensure that Stream buffer doesn't go over 60 bytes
+
+  currentMillis = millis();
+  if (currentMillis - previousMillis > samplingInterval) {
+    previousMillis += samplingInterval;
+    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
+    for (pin = 0; pin < TOTAL_PINS; pin++) {
+      if (IS_PIN_ANALOG(pin) && pinConfig[pin] == ANALOG) {
+        analogPin = PIN_TO_ANALOG(pin);
+        if (analogInputsToReport & (1 << analogPin)) {
+          Firmata.sendAnalog(analogPin, analogRead(analogPin));
+        }
+      }
+    }
+    // report i2c data for all device with read continuous mode enabled
+    if (queryIndex > -1) {
+      for (byte i = 0; i < queryIndex + 1; i++) {
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes);
+      }
+    }
+  }
+
+  checkSerial();
+
+#if !defined local_ip
+  if (Ethernet.maintain())
+  {
+    stream.maintain(Ethernet.localIP());
+  }
+#endif
+
+}
diff --git a/examples/StandardFirmataPlus/LICENSE.txt b/examples/StandardFirmataPlus/LICENSE.txt
new file mode 100644
index 00000000..77cec6dd
--- /dev/null
+++ b/examples/StandardFirmataPlus/LICENSE.txt
@@ -0,0 +1,458 @@
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
diff --git a/examples/StandardFirmataPlus/StandardFirmataPlus.ino b/examples/StandardFirmataPlus/StandardFirmataPlus.ino
new file mode 100644
index 00000000..4eb1d46e
--- /dev/null
+++ b/examples/StandardFirmataPlus/StandardFirmataPlus.ino
@@ -0,0 +1,1040 @@
+/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please clink on the following link
+  to open the list of Firmata client libraries your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2009-2015 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated by Jeff Hoefs: October 4th, 2015
+*/
+
+/*
+  README
+
+  StandardFirmataPlus adds additional features that may exceed the Flash and
+  RAM sizes of Arduino boards such as ATMega328p (Uno) and ATMega32u4
+  (Leonardo, Micro, Yun, etc). It is best to use StandardFirmataPlus with higher
+  memory boards such as the Arduino Mega, Arduino Due, Teensy 3.0/3.1/3.2.
+
+  All Firmata examples that are appended with "Plus" add the following features:
+
+  - Ability to interface with serial devices using UART, USART, or SoftwareSerial
+    depending on the capatilities of the board.
+
+  At the time of this writing, StandardFirmataPlus will still compile and run
+  on ATMega328p and ATMega32u4-based boards, but future versions of this sketch
+  may not as new features are added.
+*/
+
+#include <Servo.h>
+#include <Wire.h>
+#include <Firmata.h>
+
+// SoftwareSerial is only supported for AVR-based boards
+// The second condition checks if the IDE is in the 1.0.x series, if so, include SoftwareSerial
+// since it should be available to all boards in that IDE.
+#if defined(ARDUINO_ARCH_AVR) || (ARDUINO >= 100 && ARDUINO < 10500)
+#include <SoftwareSerial.h>
+#endif
+#include "utility/serialUtils.h"
+
+#define I2C_WRITE                   B00000000
+#define I2C_READ                    B00001000
+#define I2C_READ_CONTINUOUSLY       B00010000
+#define I2C_STOP_READING            B00011000
+#define I2C_READ_WRITE_MODE_MASK    B00011000
+#define I2C_10BIT_ADDRESS_MODE_MASK B00100000
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   10
+
+
+/*==============================================================================
+ * GLOBAL VARIABLES
+ *============================================================================*/
+
+/* analog inputs */
+int analogInputsToReport = 0; // bitwise array to store pin reporting
+
+/* digital input ports */
+byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
+byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
+
+/* pins configuration */
+byte pinConfig[TOTAL_PINS];         // configuration of every pin
+byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
+int pinState[TOTAL_PINS];           // any value that has been written
+
+/* timer variables */
+unsigned long currentMillis;        // store the current value from millis()
+unsigned long previousMillis;       // for comparison with currentMillis
+unsigned int samplingInterval = 19; // how often to run the main loop (in ms)
+
+/* serial message */
+Stream *swSerial0 = NULL;
+Stream *swSerial1 = NULL;
+Stream *swSerial2 = NULL;
+Stream *swSerial3 = NULL;
+
+byte reportSerial[MAX_SERIAL_PORTS];
+int serialBytesToRead[SERIAL_READ_ARR_LEN];
+signed char serialIndex;
+
+/* i2c data */
+struct i2c_device_info {
+  byte addr;
+  int reg;
+  byte bytes;
+};
+
+/* for i2c read continuous more */
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[32];
+boolean isI2CEnabled = false;
+signed char queryIndex = -1;
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
+
+Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
+/*==============================================================================
+ * FUNCTIONS
+ *============================================================================*/
+
+// get a pointer to the serial port associated with the specified port id
+Stream* getPortFromId(byte portId)
+{
+  switch (portId) {
+    case HW_SERIAL0:
+      // block use of Serial (typically pins 0 and 1) until ability to reclaim Serial is implemented
+      //return &Serial;
+      return NULL;
+#if defined(PIN_SERIAL1_RX)
+    case HW_SERIAL1:
+      return &Serial1;
+#endif
+#if defined(PIN_SERIAL2_RX)
+    case HW_SERIAL2:
+      return &Serial2;
+#endif
+#if defined(PIN_SERIAL3_RX)
+    case HW_SERIAL3:
+      return &Serial3;
+#endif
+#if defined(SoftwareSerial_h)
+    case SW_SERIAL0:
+      if (swSerial0 != NULL) {
+        // instances of SoftwareSerial are already pointers so simply return the instance
+        return swSerial0;
+      }
+      break;
+    case SW_SERIAL1:
+      if (swSerial1 != NULL) {
+        return swSerial1;
+      }
+      break;
+    case SW_SERIAL2:
+      if (swSerial2 != NULL) {
+        return swSerial2;
+      }
+      break;
+    case SW_SERIAL3:
+      if (swSerial3 != NULL) {
+        return swSerial3;
+      }
+      break;
+#endif
+  }
+  return NULL;
+}
+
+// Check serial ports that have READ_CONTINUOUS mode set and relay any data
+// for each port to the device attached to that port.
+void checkSerial()
+{
+  byte portId, serialData;
+  int bytesToRead = 0;
+  int numBytesToRead = 0;
+  Stream* serialPort;
+
+  if (serialIndex > -1) {
+
+    // loop through all reporting (READ_CONTINUOUS) serial ports
+    for (byte i = 0; i < serialIndex + 1; i++) {
+      portId = reportSerial[i];
+      bytesToRead = serialBytesToRead[portId];
+      serialPort = getPortFromId(portId);
+      if (serialPort == NULL) {
+        continue;
+      }
+#if defined(SoftwareSerial_h)
+      // only the SoftwareSerial port that is "listening" can read data
+      if (portId > 7 && !((SoftwareSerial*)serialPort)->isListening()) {
+        continue;
+      }
+#endif
+      if (serialPort->available() > 0) {
+        Firmata.write(START_SYSEX);
+        Firmata.write(SERIAL_MESSAGE);
+        Firmata.write(SERIAL_REPLY | portId);
+
+        if (bytesToRead == 0 || (serialPort->available() <= bytesToRead)) {
+          numBytesToRead = serialPort->available();
+        } else {
+          numBytesToRead = bytesToRead;
+        }
+
+        // relay serial data to the serial device
+        while (numBytesToRead > 0) {
+          serialData = serialPort->read();
+          Firmata.write(serialData & 0x7F);
+          Firmata.write((serialData >> 7) & 0x7F);
+          numBytesToRead--;
+        }
+        Firmata.write(END_SYSEX);
+      }
+
+    }
+  }
+}
+
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes) {
+  // allow I2C requests that don't require a register read
+  // for example, some devices using an interrupt pin to signify new data available
+  // do not always require the register read so upon interrupt you call Wire.requestFrom()
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
+    Wire.beginTransmission(address);
+    wireWrite((byte)theRegister);
+    Wire.endTransmission();
+    // do not set a value of 0
+    if (i2cReadDelayTime > 0) {
+      // delay is necessary for some devices such as WiiNunchuck
+      delayMicroseconds(i2cReadDelayTime);
+    }
+  } else {
+    theRegister = 0;  // fill the register with a dummy value
+  }
+
+  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
+
+  // check to be sure correct number of bytes were returned by slave
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
+  }
+
+  // send slave address, register and received bytes
+  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
+}
+
+void outputPort(byte portNumber, byte portValue, byte forceSend)
+{
+  // pins not configured as INPUT are cleared to zeros
+  portValue = portValue & portConfigInputs[portNumber];
+  // only send if the value is different than previously sent
+  if (forceSend || previousPINs[portNumber] != portValue) {
+    Firmata.sendDigitalPort(portNumber, portValue);
+    previousPINs[portNumber] = portValue;
+  }
+}
+
+/* -----------------------------------------------------------------------------
+ * check all the active digital inputs for change of state, then add any events
+ * to the Serial output queue using Serial.print() */
+void checkDigitalInputs(void)
+{
+  /* Using non-looping code allows constants to be given to readPort().
+   * The compiler will apply substantial optimizations if the inputs
+   * to readPort() are compile-time constants. */
+  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
+  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
+  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
+  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
+  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
+  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
+  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
+  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
+  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
+  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
+  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
+  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
+  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
+  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
+  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
+  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
+}
+
+// -----------------------------------------------------------------------------
+/* sets the pin mode to the correct state and sets the relevant bits in the
+ * two bit-arrays that track Digital I/O and PWM status
+ */
+void setPinModeCallback(byte pin, int mode)
+{
+  if (pinConfig[pin] == IGNORE)
+    return;
+
+  if (pinConfig[pin] == I2C && isI2CEnabled && mode != I2C) {
+    // disable i2c so pins can be used for other functions
+    // the following if statements should reconfigure the pins properly
+    disableI2CPins();
+  }
+  if (IS_PIN_DIGITAL(pin) && mode != SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
+  }
+  if (IS_PIN_ANALOG(pin)) {
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == ANALOG ? 1 : 0); // turn on/off reporting
+  }
+  if (IS_PIN_DIGITAL(pin)) {
+    if (mode == INPUT) {
+      portConfigInputs[pin / 8] |= (1 << (pin & 7));
+    } else {
+      portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+    }
+  }
+  pinState[pin] = 0;
+  switch (mode) {
+    case ANALOG:
+      if (IS_PIN_ANALOG(pin)) {
+        if (IS_PIN_DIGITAL(pin)) {
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+        }
+        pinConfig[pin] = ANALOG;
+      }
+      break;
+    case INPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+        pinConfig[pin] = INPUT;
+      }
+      break;
+    case OUTPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
+        pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+        pinConfig[pin] = OUTPUT;
+      }
+      break;
+    case PWM:
+      if (IS_PIN_PWM(pin)) {
+        pinMode(PIN_TO_PWM(pin), OUTPUT);
+        analogWrite(PIN_TO_PWM(pin), 0);
+        pinConfig[pin] = PWM;
+      }
+      break;
+    case SERVO:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinConfig[pin] = SERVO;
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case I2C:
+      if (IS_PIN_I2C(pin)) {
+        // mark the pin as i2c
+        // the user must call I2C_CONFIG to enable I2C for a device
+        pinConfig[pin] = I2C;
+      }
+      break;
+    case MODE_SERIAL:
+      // used for both HW and SW serial
+      pinConfig[pin] = MODE_SERIAL;
+      break;
+    default:
+      Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+void analogWriteCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS) {
+    switch (pinConfig[pin]) {
+      case SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        pinState[pin] = value;
+        break;
+      case PWM:
+        if (IS_PIN_PWM(pin))
+          analogWrite(PIN_TO_PWM(pin), value);
+        pinState[pin] = value;
+        break;
+    }
+  }
+}
+
+void digitalWriteCallback(byte port, int value)
+{
+  byte pin, lastPin, mask = 1, pinWriteMask = 0;
+
+  if (port < TOTAL_PORTS) {
+    // create a mask of the pins on this port that are writable.
+    lastPin = port * 8 + 8;
+    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
+    for (pin = port * 8; pin < lastPin; pin++) {
+      // do not disturb non-digital pins (eg, Rx & Tx)
+      if (IS_PIN_DIGITAL(pin)) {
+        // only write to OUTPUT and INPUT (enables pullup)
+        // do not touch pins in PWM, ANALOG, SERVO or other modes
+        if (pinConfig[pin] == OUTPUT || pinConfig[pin] == INPUT) {
+          pinWriteMask |= mask;
+          pinState[pin] = ((byte)value & mask) ? 1 : 0;
+        }
+      }
+      mask = mask << 1;
+    }
+    writePort(port, (byte)value, pinWriteMask);
+  }
+}
+
+
+// -----------------------------------------------------------------------------
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+void reportDigitalCallback(byte port, int value)
+{
+  if (port < TOTAL_PORTS) {
+    reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
+  }
+  // do not disable analog reporting on these 8 pins, to allow some
+  // pins used for digital, others analog.  Instead, allow both types
+  // of reporting to be enabled, but check if the pin is configured
+  // as analog when sampling the analog inputs.  Likewise, while
+  // scanning digital pins, portConfigInputs will mask off values from any
+  // pins configured as analog
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  byte mode;
+  byte slaveAddress;
+  byte data;
+  int slaveRegister;
+  unsigned int delayTime;
+
+  switch (command) {
+    case I2C_REQUEST:
+      mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
+      if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
+        Firmata.sendString("10-bit addressing not supported");
+        return;
+      }
+      else {
+        slaveAddress = argv[0];
+      }
+
+      switch (mode) {
+        case I2C_WRITE:
+          Wire.beginTransmission(slaveAddress);
+          for (byte i = 2; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            wireWrite(data);
+          }
+          Wire.endTransmission();
+          delayMicroseconds(70);
+          break;
+        case I2C_READ:
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          readAndReportData(slaveAddress, (int)slaveRegister, data);
+          break;
+        case I2C_READ_CONTINUOUSLY:
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
+            // too many queries, just ignore
+            Firmata.sendString("too many queries");
+            break;
+          }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          queryIndex++;
+          query[queryIndex].addr = slaveAddress;
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          break;
+        case I2C_STOP_READING:
+          byte queryIndexToSkip;
+          // if read continuous mode is enabled for only 1 i2c device, disable
+          // read continuous reporting for that device
+          if (queryIndex <= 0) {
+            queryIndex = -1;
+          } else {
+            // if read continuous mode is enabled for multiple devices,
+            // determine which device to stop reading and remove it's data from
+            // the array, shifiting other array data to fill the space
+            for (byte i = 0; i < queryIndex + 1; i++) {
+              if (query[i].addr == slaveAddress) {
+                queryIndexToSkip = i;
+                break;
+              }
+            }
+
+            for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
+              if (i < I2C_MAX_QUERIES) {
+                query[i].addr = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
+                query[i].bytes = query[i + 1].bytes;
+              }
+            }
+            queryIndex--;
+          }
+          break;
+        default:
+          break;
+      }
+      break;
+    case I2C_CONFIG:
+      delayTime = (argv[0] + (argv[1] << 7));
+
+      if (delayTime > 0) {
+        i2cReadDelayTime = delayTime;
+      }
+
+      if (!isI2CEnabled) {
+        enableI2CPins();
+      }
+
+      break;
+    case SERVO_CONFIG:
+      if (argc > 4) {
+        // these vars are here for clarity, they'll optimized away by the compiler
+        byte pin = argv[0];
+        int minPulse = argv[1] + (argv[2] << 7);
+        int maxPulse = argv[3] + (argv[4] << 7);
+
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, SERVO);
+        }
+      }
+      break;
+    case SAMPLING_INTERVAL:
+      if (argc > 1) {
+        samplingInterval = argv[0] + (argv[1] << 7);
+        if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+          samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+        }
+      } else {
+        //Firmata.sendString("Not enough data");
+      }
+      break;
+    case EXTENDED_ANALOG:
+      if (argc > 1) {
+        int val = argv[1];
+        if (argc > 2) val |= (argv[2] << 7);
+        if (argc > 3) val |= (argv[3] << 14);
+        analogWriteCallback(argv[0], val);
+      }
+      break;
+    case CAPABILITY_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(CAPABILITY_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write((byte)INPUT);
+          Firmata.write(1);
+          Firmata.write((byte)OUTPUT);
+          Firmata.write(1);
+        }
+        if (IS_PIN_ANALOG(pin)) {
+          Firmata.write(ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
+        }
+        if (IS_PIN_PWM(pin)) {
+          Firmata.write(PWM);
+          Firmata.write(8); // 8 = 8-bit resolution
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(SERVO);
+          Firmata.write(14);
+        }
+        if (IS_PIN_I2C(pin)) {
+          Firmata.write(I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+        if (IS_PIN_SERIAL(pin)) {
+          Firmata.write(MODE_SERIAL);
+          Firmata.write(getSerialPinType(pin));
+        }
+        Firmata.write(127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+    case PIN_STATE_QUERY:
+      if (argc > 0) {
+        byte pin = argv[0];
+        Firmata.write(START_SYSEX);
+        Firmata.write(PIN_STATE_RESPONSE);
+        Firmata.write(pin);
+        if (pin < TOTAL_PINS) {
+          Firmata.write((byte)pinConfig[pin]);
+          Firmata.write((byte)pinState[pin] & 0x7F);
+          if (pinState[pin] & 0xFF80) Firmata.write((byte)(pinState[pin] >> 7) & 0x7F);
+          if (pinState[pin] & 0xC000) Firmata.write((byte)(pinState[pin] >> 14) & 0x7F);
+        }
+        Firmata.write(END_SYSEX);
+      }
+      break;
+    case ANALOG_MAPPING_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(ANALOG_MAPPING_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        Firmata.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+
+    case SERIAL_MESSAGE:
+      Stream * serialPort;
+      mode = argv[0] & SERIAL_MODE_MASK;
+      byte portId = argv[0] & SERIAL_PORT_ID_MASK;
+
+      switch (mode) {
+        case SERIAL_CONFIG:
+          {
+            long baud = (long)argv[1] | ((long)argv[2] << 7) | ((long)argv[3] << 14);
+            byte txPin, rxPin;
+            serial_pins pins;
+
+            if (portId > 7 && argc > 4) {
+              rxPin = argv[4];
+              txPin = argv[5];
+            }
+
+            if (portId < 8) {
+              serialPort = getPortFromId(portId);
+              if (serialPort != NULL) {
+                pins = getSerialPinNumbers(portId);
+                if (pins.rx != 0 && pins.tx != 0) {
+                  setPinModeCallback(pins.rx, MODE_SERIAL);
+                  setPinModeCallback(pins.tx, MODE_SERIAL);
+                  // Fixes an issue where some serial devices would not work properly with Arduino Due
+                  // because all Arduino pins are set to OUTPUT by default in StandardFirmata.
+                  pinMode(pins.rx, INPUT);
+                }
+                ((HardwareSerial*)serialPort)->begin(baud);
+              }
+            } else {
+#if defined(SoftwareSerial_h)
+              switch (portId) {
+                case SW_SERIAL0:
+                  if (swSerial0 == NULL) {
+                    swSerial0 = new SoftwareSerial(rxPin, txPin);
+                  }
+                  break;
+                case SW_SERIAL1:
+                  if (swSerial1 == NULL) {
+                    swSerial1 = new SoftwareSerial(rxPin, txPin);
+                  }
+                  break;
+                case SW_SERIAL2:
+                  if (swSerial2 == NULL) {
+                    swSerial2 = new SoftwareSerial(rxPin, txPin);
+                  }
+                  break;
+                case SW_SERIAL3:
+                  if (swSerial3 == NULL) {
+                    swSerial3 = new SoftwareSerial(rxPin, txPin);
+                  }
+                  break;
+              }
+              serialPort = getPortFromId(portId);
+              if (serialPort != NULL) {
+                setPinModeCallback(rxPin, MODE_SERIAL);
+                setPinModeCallback(txPin, MODE_SERIAL);
+                ((SoftwareSerial*)serialPort)->begin(baud);
+              }
+#endif
+            }
+            break; // SERIAL_CONFIG
+          }
+        case SERIAL_WRITE:
+          {
+            byte data;
+            serialPort = getPortFromId(portId);
+            if (serialPort == NULL) {
+              break;
+            }
+            for (byte i = 1; i < argc; i += 2) {
+              data = argv[i] + (argv[i + 1] << 7);
+              serialPort->write(data);
+            }
+            break; // SERIAL_WRITE
+          }
+        case SERIAL_READ:
+          if (argv[1] == SERIAL_READ_CONTINUOUSLY) {
+            if (serialIndex + 1 >= MAX_SERIAL_PORTS) {
+              break;
+            }
+
+            if (argc > 2) {
+              // maximum number of bytes to read from buffer per iteration of loop()
+              serialBytesToRead[portId] = (int)argv[2] | ((int)argv[3] << 7);
+            } else {
+              // read all available bytes per iteration of loop()
+              serialBytesToRead[portId] = 0;
+            }
+            serialIndex++;
+            reportSerial[serialIndex] = portId;
+          } else if (argv[1] == SERIAL_STOP_READING) {
+            byte serialIndexToSkip;
+            if (serialIndex <= 0) {
+              serialIndex = -1;
+            } else {
+              for (byte i = 0; i < serialIndex + 1; i++) {
+                if (reportSerial[i] == portId) {
+                  serialIndexToSkip = i;
+                  break;
+                }
+              }
+              // shift elements over to fill space left by removed element
+              for (byte i = serialIndexToSkip; i < serialIndex + 1; i++) {
+                if (i < MAX_SERIAL_PORTS) {
+                  reportSerial[i] = reportSerial[i + 1];
+                }
+              }
+              serialIndex--;
+            }
+          }
+          break; // SERIAL_READ
+        case SERIAL_CLOSE:
+          serialPort = getPortFromId(portId);
+          if (serialPort != NULL) {
+            if (portId < 8) {
+              ((HardwareSerial*)serialPort)->end();
+            } else {
+#if defined(SoftwareSerial_h)
+              ((SoftwareSerial*)serialPort)->end();
+              if (serialPort != NULL) {
+                free(serialPort);
+                serialPort = NULL;
+              }
+#endif
+            }
+          }
+          break; // SERIAL_CLOSE
+        case SERIAL_FLUSH:
+          serialPort = getPortFromId(portId);
+          if (serialPort != NULL) {
+            getPortFromId(portId)->flush();
+          }
+          break; // SERIAL_FLUSH
+#if defined(SoftwareSerial_h)
+        case SERIAL_LISTEN:
+          // can only call listen() on software serial ports
+          if (portId > 7) {
+            serialPort = getPortFromId(portId);
+            if (serialPort != NULL) {
+              ((SoftwareSerial*)serialPort)->listen();
+            }
+          }
+          break; // SERIAL_LISTEN
+#endif
+      }
+      break;
+  }
+}
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+/*==============================================================================
+ * SETUP()
+ *============================================================================*/
+
+void systemResetCallback()
+{
+  Stream *serialPort;
+  isResetting = true;
+
+  // initialize a defalt state
+  // TODO: option to load config from EEPROM instead of default
+
+  if (isI2CEnabled) {
+    disableI2CPins();
+  }
+
+#if defined(SoftwareSerial_h)
+  // free memory allocated for SoftwareSerial ports
+  for (byte i = SW_SERIAL0; i < SW_SERIAL3 + 1; i++) {
+    serialPort = getPortFromId(i);
+    if (serialPort != NULL) {
+      free(serialPort);
+      serialPort = NULL;
+    }
+  }
+#endif
+
+  serialIndex = -1;
+  for (byte i = 0; i < SERIAL_READ_ARR_LEN; i++) {
+    serialBytesToRead[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PORTS; i++) {
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
+    previousPINs[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
+    if (IS_PIN_ANALOG(i)) {
+      // turns off pullup, configures everything
+      setPinModeCallback(i, ANALOG);
+    } else {
+      // sets the output to 0, configures portConfigInputs
+      setPinModeCallback(i, OUTPUT);
+    }
+
+    servoPinMap[i] = 255;
+  }
+  // by default, do not report any analog inputs
+  analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
+
+  /* send digital inputs to set the initial state on the host computer,
+   * since once in the loop(), this firmware will only send on change */
+  /*
+  TODO: this can never execute, since no pins default to digital input
+        but it will be needed when/if we support EEPROM stored config
+  for (byte i=0; i < TOTAL_PORTS; i++) {
+    outputPort(i, readPort(i, portConfigInputs[i]), true);
+  }
+  */
+  isResetting = false;
+}
+
+void setup()
+{
+  Firmata.setFirmwareVersion(FIRMATA_MAJOR_VERSION, FIRMATA_MINOR_VERSION);
+
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
+  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
+  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
+  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(START_SYSEX, sysexCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
+
+  // to use a port other than Serial, such as Serial1 on an Arduino Leonardo or Mega,
+  // Call begin(baud) on the alternate serial port and pass it to Firmata to begin like this:
+  // Serial1.begin(57600);
+  // Firmata.begin(Serial1);
+  // However do not do this if you are using SERIAL_MESSAGE
+
+  Firmata.begin(57600);
+  while (!Serial) {
+    ; // wait for serial port to connect. Only needed for ATmega32u4-based boards (Leonardo, etc).
+  }
+  systemResetCallback();  // reset to default config
+}
+
+/*==============================================================================
+ * LOOP()
+ *============================================================================*/
+void loop()
+{
+  byte pin, analogPin;
+
+  /* DIGITALREAD - as fast as possible, check for changes and output them to the
+   * FTDI buffer using Serial.print()  */
+  checkDigitalInputs();
+
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
+   * checking digital inputs.  */
+  while (Firmata.available())
+    Firmata.processInput();
+
+  // TODO - ensure that Stream buffer doesn't go over 60 bytes
+
+  currentMillis = millis();
+  if (currentMillis - previousMillis > samplingInterval) {
+    previousMillis += samplingInterval;
+    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
+    for (pin = 0; pin < TOTAL_PINS; pin++) {
+      if (IS_PIN_ANALOG(pin) && pinConfig[pin] == ANALOG) {
+        analogPin = PIN_TO_ANALOG(pin);
+        if (analogInputsToReport & (1 << analogPin)) {
+          Firmata.sendAnalog(analogPin, analogRead(analogPin));
+        }
+      }
+    }
+    // report i2c data for all device with read continuous mode enabled
+    if (queryIndex > -1) {
+      for (byte i = 0; i < queryIndex + 1; i++) {
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes);
+      }
+    }
+  }
+
+  checkSerial();
+}
diff --git a/examples/StandardFirmataYun/LICENSE.txt b/examples/StandardFirmataYun/LICENSE.txt
new file mode 100755
index 00000000..77cec6dd
--- /dev/null
+++ b/examples/StandardFirmataYun/LICENSE.txt
@@ -0,0 +1,458 @@
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+

From d78d505ef4ccebe121e6f0d08e56c78493794717 Mon Sep 17 00:00:00 2001
From: Jeff Hoefs <jeff.hoefs@gmail.com>
Date: Mon, 5 Oct 2015 23:13:21 -0700
Subject: [PATCH 10/10] Uno and ArduinoEthnet boards still work with
 StandardFirmataEthernetPlus

---
 .../StandardFirmataEthernet.ino               |  3 +++
 .../StandardFirmataEthernetPlus.ino           | 21 ++++++++++++-------
 2 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/examples/StandardFirmataEthernet/StandardFirmataEthernet.ino b/examples/StandardFirmataEthernet/StandardFirmataEthernet.ino
index 4f693d20..7391a6ac 100644
--- a/examples/StandardFirmataEthernet/StandardFirmataEthernet.ino
+++ b/examples/StandardFirmataEthernet/StandardFirmataEthernet.ino
@@ -43,6 +43,9 @@
   - Arduino Mega: (D4, D10, D50, D51, D52, D53)
   - Arduino Leonardo: (D4, D10)
   - Arduino Due: (D4, D10)
+
+  If you are using an ArduinoEthernet board, the following pins cannot be used (same as Uno):
+  - D4, D10, D11, D12, D13
 */
 
 #include <Servo.h>
diff --git a/examples/StandardFirmataEthernetPlus/StandardFirmataEthernetPlus.ino b/examples/StandardFirmataEthernetPlus/StandardFirmataEthernetPlus.ino
index 1062249c..c4e8f380 100644
--- a/examples/StandardFirmataEthernetPlus/StandardFirmataEthernetPlus.ino
+++ b/examples/StandardFirmataEthernetPlus/StandardFirmataEthernetPlus.ino
@@ -28,17 +28,18 @@
 
   StandardFirmataEthernetPlus adds additional features that may exceed the Flash and
   RAM sizes of Arduino boards such as ATMega328p (Uno) and ATMega32u4
-  (Leonardo, Micro, Yun, etc). It is best to use StandardFirmataPlus with higher
-  memory boards such as the Arduino Mega, Arduino Due, Teensy 3.0/3.1/3.2, etc.
+  (Leonardo, Micro, Yun, etc). It is best to use StandardFirmataPlus with a board that
+  has > 32k Flash and > 3k RAM such as: Arduino Mega, Arduino Due, Teensy 3.0/3.1/3.2, etc.
 
   This sketch consumes too much Flash and RAM to run reliably on an
-  Arduino Ethernet board, Arduino Leonardo, Yun, Uno or other ATMega328p or
-  ATMega32u4-based boards. You will need a board with > 32k Flash and 3k RAM.
-  Use StandardFirmataEthernet.ino instead for those boards and other boards that do not
-  meet the Flash and RAM requirements.
+  Arduino Leonardo, Yun, ATMega32u4-based board. Use StandardFirmataEthernet.ino instead
+  for those boards and other boards that do not meet the Flash and RAM requirements.
 
-  USE: To use StandardFirmataEthernetPlus you will need to have an Arduino Ethernet shield
-  (or clone).
+  To use StandardFirmataEthernet you will need to have one of the following
+  boards or shields:
+
+  - Arduino Ethernet shield (or clone)
+  - Arduino Ethernet board (or clone)
 
   Follow the instructions in the NETWORK CONFIGURATION section below to
   configure your particular hardware.
@@ -48,6 +49,10 @@
 
   - Arduino Mega: (D4, D10, D50, D51, D52, D53)
   - Arduino Due: (D4, D10)
+  - Arduino Uno or other ATMega328p boards: (D4, D10, D11, D12, D13)
+
+  If you are using an ArduinoEthernet board, the following pins cannot be used (same as Uno):
+  - D4, D10, D11, D12, D13
 */
 
 #include <Servo.h>