From 7d6ada1db3c630265c60b661a55995cfe0c9a3a0 Mon Sep 17 00:00:00 2001
From: "Jens B." <jnsbyr@gmail.com>
Date: Mon, 18 Dec 2023 23:31:23 +0100
Subject: [PATCH 01/12] [WiFi] added support for ESP32 architecture and XIAO
 ESP32C3 board - support for ESP32 architecture with WiFi interface
 (WiFiStream, Firmata.h, StandardFirmataWiFi) - support for Seed Studio XIAO
 ESP32C3 board (Board.h) - support for Firmata applications without Servo.h
 (Boards.h, StandardFirmataWiFi)

---
 Boards.h                                      | 37 ++++++++++++++-
 Firmata.h                                     |  6 ++-
 FirmataConstants.h                            |  2 +-
 .../StandardFirmataWiFi.ino                   | 46 ++++++++++++-------
 examples/StandardFirmataWiFi/wifiConfig.h     | 29 +++++++++++-
 library.properties                            |  2 +-
 utility/WiFiStream.h                          | 20 +++++---
 7 files changed, 111 insertions(+), 31 deletions(-)

diff --git a/Boards.h b/Boards.h
index f6cc0422..0404d562 100644
--- a/Boards.h
+++ b/Boards.h
@@ -2,6 +2,7 @@
   Boards.h - Hardware Abstraction Layer for Firmata library
   Copyright (c) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
   Copyright (C) 2009-2017 Jeff Hoefs.  All rights reserved.
+  Copyright (C) 2023 Jens B. 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
@@ -10,7 +11,7 @@
 
   See file LICENSE.txt for further informations on licensing terms.
 
-  Last updated April 15th, 2018
+  Last updated December 17th, 2023
 */
 
 #ifndef Firmata_Boards_h
@@ -29,7 +30,16 @@
 // compile, but without support for any Servos.  Hopefully that's what the
 // user intended by not including Servo.h
 #ifndef MAX_SERVOS
-#define MAX_SERVOS 0
+  #define MAX_SERVOS 0
+  class Servo
+  {
+    public:
+      uint8_t attach(int pin) { return 0; };
+      uint8_t attach(int pin, int min, int max) { return 0; };
+      void detach() {};
+      void write(int value) {};
+      bool attached() { return false; };
+  };
 #endif
 
 /*
@@ -1021,6 +1031,29 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 #define PIN_TO_SERVO(p)         (p)
 #define DEFAULT_PWM_RESOLUTION  10
 
+// XIAO ESP32C2
+// note: Firmata pin numbering schema is by ESP32 GPIO -> IS_XXX checks GPIO number
+#elif defined(ARDUINO_XIAO_ESP32C3)
+#define TOTAL_ANALOG_PINS       (A2 + 1)          // (max GPIOx + 1), there are 4 physical analog pins but only 3 are supported by ESP32 SDK 2.0.14 via ADC1
+#define TOTAL_PINS              NUM_DIGITAL_PINS  // (max GPIOx + 1), there are 11 physical pins
+#define PIN_SERIAL_RX           RX
+#define PIN_SERIAL_TX           TX
+#define IS_PIN_DIGITAL(p)       (((p) >= D0 && (p) <= D10) || (p) == D6 || (p) == D7)
+#define IS_PIN_ANALOG(p)        ((p) >= A0 && (p) <= A2)
+#define IS_PIN_PWM(p)           0
+#define IS_PIN_SERVO(p)         (IS_PIN_DIGITAL(p) && MAX_SERVOS > 0)
+#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_INTERRUPT(p)     (digitalPinToInterrupt(p) > NOT_AN_INTERRUPT)
+#define IS_PIN_SERIAL(p)        ((p) == PIN_SERIAL_RX || (p) == PIN_SERIAL_TX)
+#define PIN_TO_DIGITAL(p)       ((p) < 6? D0 + (p) : ((p) < 8? D6 + 6 - (p) : (p))) // Dx to GPIOy
+#define PIN_TO_ANALOG(p)        (p)                                                 // FIRMATAx to GPIOy
+#define PIN_TO_PWM(p)           127                                                 // @TODO ESP32 SDK does not support analogWrite()
+#define PIN_TO_SERVO(p)         127                                                 // @TODO ESP32 SDK does not support servos
+
+#define DEFAULT_PWM_RESOLUTION     8  // see esp32-hal-led.c, analog_resolution
+#define DEFAULT_ANALOG_RESOLUTION 12  // see esp32-hal-adc.h, analogSetWidth()
+
 // STM32 based boards
 #elif defined(ARDUINO_ARCH_STM32)
 #define TOTAL_ANALOG_PINS       NUM_ANALOG_INPUTS
diff --git a/Firmata.h b/Firmata.h
index fb993d79..08125f6c 100644
--- a/Firmata.h
+++ b/Firmata.h
@@ -1,5 +1,5 @@
 /*
-  Firmata.h - Firmata library v2.5.8 - 2018-04-15
+  Firmata.h - Firmata library v2.5.10 - 2023-12-16
   Copyright (c) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
   Copyright (C) 2009-2017 Jeff Hoefs.  All rights reserved.
 
@@ -38,7 +38,9 @@
 //#define INPUT                 0x00 // defined in Arduino.h
 //#define OUTPUT                0x01 // defined in Arduino.h
 // DEPRECATED as of Firmata v2.5
-#define ANALOG                  0x02 // same as PIN_MODE_ANALOG
+#ifndef ARDUINO_ARCH_ESP32
+  #define ANALOG                0x02 // same as PIN_MODE_ANALOG
+#endif
 #define PWM                     0x03 // same as PIN_MODE_PWM
 #define SERVO                   0x04 // same as PIN_MODE_SERVO
 #define SHIFT                   0x05 // same as PIN_MODE_SHIFT
diff --git a/FirmataConstants.h b/FirmataConstants.h
index dee84079..a7459676 100644
--- a/FirmataConstants.h
+++ b/FirmataConstants.h
@@ -21,7 +21,7 @@ namespace firmata {
  */
 static const int FIRMWARE_MAJOR_VERSION =  2;
 static const int FIRMWARE_MINOR_VERSION =  5;
-static const int FIRMWARE_BUGFIX_VERSION = 7;
+static const int FIRMWARE_BUGFIX_VERSION = 10;
 
 /* Version numbers for the protocol.  The protocol is still changing, so these
  * version numbers are important.
diff --git a/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino b/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino
index 98b33587..0fc68481 100644
--- a/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino
+++ b/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino
@@ -22,7 +22,7 @@
 
   See file LICENSE.txt for further informations on licensing terms.
 
-  Last updated August 17th, 2017
+  Last updated December 17th, 2023
 */
 
 /*
@@ -74,7 +74,11 @@
   - Arduino Mega: (D5, D7, D10, D50, D52, D53)
 */
 
-#include <Servo.h>
+#ifndef ARDUINO_ARCH_ESP32
+  // NOTE: ESP32 SKD does not provide an implementation for class Servo
+  //       -> requires a skeleton implementation for Servo in Boards.h to be able to compile
+  #include <Servo.h>
+#endif
 #include <Wire.h>
 #include <Firmata.h>
 
@@ -235,8 +239,10 @@ void detachServo(byte pin)
   } 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];
+    if (detachedServoCount < MAX_SERVOS) {
+      detachedServos[detachedServoCount] = servoPinMap[pin];
+      detachedServoCount++;
+    }
   }
 
   servoPinMap[pin] = 255;
@@ -370,7 +376,7 @@ void setPinModeCallback(byte pin, int mode)
     reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
   }
   if (IS_PIN_DIGITAL(pin)) {
-    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+    if (mode == PIN_MODE_INPUT || mode == PIN_MODE_PULLUP) {
       portConfigInputs[pin / 8] |= (1 << (pin & 7));
     } else {
       portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
@@ -390,14 +396,14 @@ void setPinModeCallback(byte pin, int mode)
         Firmata.setPinMode(pin, PIN_MODE_ANALOG);
       }
       break;
-    case INPUT:
+    case PIN_MODE_INPUT:
       if (IS_PIN_DIGITAL(pin)) {
         pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
 #if ARDUINO <= 100
         // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
         digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
 #endif
-        Firmata.setPinMode(pin, INPUT);
+        Firmata.setPinMode(pin, PIN_MODE_INPUT);
       }
       break;
     case PIN_MODE_PULLUP:
@@ -407,14 +413,14 @@ void setPinModeCallback(byte pin, int mode)
         Firmata.setPinState(pin, 1);
       }
       break;
-    case OUTPUT:
+    case PIN_MODE_OUTPUT:
       if (IS_PIN_DIGITAL(pin)) {
         if (Firmata.getPinMode(pin) == PIN_MODE_PWM) {
           // Disable PWM if pin mode was previously set to PWM.
           digitalWrite(PIN_TO_DIGITAL(pin), LOW);
         }
         pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
-        Firmata.setPinMode(pin, OUTPUT);
+        Firmata.setPinMode(pin, PIN_MODE_OUTPUT);
       }
       break;
     case PIN_MODE_PWM:
@@ -461,7 +467,7 @@ void setPinModeCallback(byte pin, int mode)
 void setPinValueCallback(byte pin, int value)
 {
   if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
-    if (Firmata.getPinMode(pin) == OUTPUT) {
+    if (Firmata.getPinMode(pin) == PIN_MODE_OUTPUT) {
       Firmata.setPinState(pin, value);
       digitalWrite(PIN_TO_DIGITAL(pin), value);
     }
@@ -498,11 +504,11 @@ void digitalWriteCallback(byte port, int value)
       // do not disturb non-digital pins (eg, Rx & Tx)
       if (IS_PIN_DIGITAL(pin)) {
         // do not touch pins in PWM, ANALOG, SERVO or other modes
-        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+        if (Firmata.getPinMode(pin) == PIN_MODE_OUTPUT || Firmata.getPinMode(pin) == PIN_MODE_INPUT) {
           pinValue = ((byte)value & mask) ? 1 : 0;
-          if (Firmata.getPinMode(pin) == OUTPUT) {
+          if (Firmata.getPinMode(pin) == PIN_MODE_OUTPUT) {
             pinWriteMask |= mask;
-          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+          } else if (Firmata.getPinMode(pin) == PIN_MODE_INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
             // only handle INPUT here for backwards compatibility
 #if ARDUINO > 100
             pinMode(pin, INPUT_PULLUP);
@@ -725,22 +731,26 @@ void sysexCallback(byte command, byte argc, byte *argv)
       Firmata.write(CAPABILITY_RESPONSE);
       for (byte pin = 0; pin < TOTAL_PINS; pin++) {
         if (IS_PIN_DIGITAL(pin)) {
-          Firmata.write((byte)INPUT);
+          Firmata.write((byte)PIN_MODE_INPUT);
           Firmata.write(1);
           Firmata.write((byte)PIN_MODE_PULLUP);
           Firmata.write(1);
-          Firmata.write((byte)OUTPUT);
+          Firmata.write((byte)PIN_MODE_OUTPUT);
           Firmata.write(1);
         }
         if (IS_PIN_ANALOG(pin)) {
           Firmata.write(PIN_MODE_ANALOG);
+        #ifdef DEFAULT_ANALOG_RESOLUTION
+          Firmata.write(DEFAULT_ANALOG_RESOLUTION);
+        #else
           Firmata.write(10); // 10 = 10-bit resolution
+        #endif
         }
         if (IS_PIN_PWM(pin)) {
           Firmata.write(PIN_MODE_PWM);
           Firmata.write(DEFAULT_PWM_RESOLUTION);
         }
-        if (IS_PIN_DIGITAL(pin)) {
+        if (IS_PIN_SERVO(pin)) {
           Firmata.write(PIN_MODE_SERVO);
           Firmata.write(14);
         }
@@ -820,7 +830,7 @@ void systemResetCallback()
       setPinModeCallback(i, PIN_MODE_ANALOG);
     } else if (IS_PIN_DIGITAL(i)) {
       // sets the output to 0, configures portConfigInputs
-      setPinModeCallback(i, OUTPUT);
+      setPinModeCallback(i, PIN_MODE_OUTPUT);
     }
 
     servoPinMap[i] = 255;
@@ -871,6 +881,7 @@ void printWifiStatus() {
     DEBUG_PRINT( "WiFi connection failed. Status value: " );
     DEBUG_PRINTLN( WiFi.status() );
   }
+#ifdef SERIAL_DEBUG
   else
   {
     // print the SSID of the network you're attached to:
@@ -888,6 +899,7 @@ void printWifiStatus() {
     DEBUG_PRINT( rssi );
     DEBUG_PRINTLN( " dBm" );
   }
+#endif
 }
 
 /*
diff --git a/examples/StandardFirmataWiFi/wifiConfig.h b/examples/StandardFirmataWiFi/wifiConfig.h
index 6cdebc84..4a4955bd 100644
--- a/examples/StandardFirmataWiFi/wifiConfig.h
+++ b/examples/StandardFirmataWiFi/wifiConfig.h
@@ -8,8 +8,8 @@
  *============================================================================*/
 
 // STEP 1 [REQUIRED]
-// Uncomment / comment the appropriate set of includes for your hardware (OPTION A, B or C)
-// Arduino MKR1000 or ESP8266 are enabled by default if compiling for either of those boards.
+// Uncomment / comment the appropriate set of includes for your hardware (OPTION A ... F)
+// Arduino MKR1000, ESP8266 and ESP32 are enabled by default if compiling for either of those boards.
 
 /*
  * OPTION A: Configure for Arduino MKR1000 or Arduino WiFi Shield 101
@@ -137,6 +137,31 @@
   #endif
 #endif
 
+/*
+ * OPTION F: Configure for ESP32
+ *
+ * This will configure StandardFirmataWiFi to use the ESP8266WiFi library for boards
+ * with an ESP32 chip.
+ *
+ * The appropriate libraries are included automatically when compiling for the ESP32 so
+ * continue on to STEP 2.
+ */
+
+#if defined(ARDUINO_ARCH_ESP32)
+  // automatically include if compiling for ESP32
+  #define ESP32_WIFI
+#endif
+#ifdef ESP32_WIFI
+  #include <WiFi.h>
+  #include "utility/WiFiClientStream.h"
+  #include "utility/WiFiServerStream.h"
+  #ifdef WIFI_LIB_INCLUDED
+    #define MULTIPLE_WIFI_LIB_INCLUDES
+  #else
+    #define WIFI_LIB_INCLUDED
+  #endif
+#endif
+
 //------------------------------
 // TODO
 //------------------------------
diff --git a/library.properties b/library.properties
index 6e27f0b8..6798b939 100644
--- a/library.properties
+++ b/library.properties
@@ -1,5 +1,5 @@
 name=Firmata
-version=2.5.9
+version=2.5.10
 author=Firmata Developers
 maintainer=Firmata team
 sentence=Enables the communication with computer apps using a standard serial protocol. For all Arduino/Genuino boards.
diff --git a/utility/WiFiStream.h b/utility/WiFiStream.h
index 1ad44bbb..b52e6d59 100644
--- a/utility/WiFiStream.h
+++ b/utility/WiFiStream.h
@@ -6,7 +6,7 @@
   are compatible with the Arduino WiFi library.
 
   Copyright (C) 2015-2016 Jesse Frush. All rights reserved.
-  Copyright (C) 2016      Jens B. All rights reserved.
+  Copyright (C) 2016 Jens B. 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
@@ -15,7 +15,7 @@
 
   See file LICENSE.txt for further informations on licensing terms.
 
-  Last updated April 23rd, 2016
+  Last updated December 17th, 2023
  */
 
 #ifndef WIFI_STREAM_H
@@ -69,7 +69,7 @@ class WiFiStream : public Stream
  *           network configuration
  ******************************************************************************/
 
-#ifndef ESP8266
+#if (!ESP8266) && (!ARDUINO_ARCH_ESP32)
   /**
    * configure a static local IP address without defining the local network
    * DHCP will be used as long as local IP address is not defined
@@ -90,7 +90,7 @@ class WiFiStream : public Stream
     _local_ip = local_ip;
     _subnet = subnet;
     _gateway = gateway;
-#ifndef ESP8266
+#if (!ESP8266) && (!ARDUINO_ARCH_ESP32)
     WiFi.config( local_ip, IPAddress(0, 0, 0, 0), gateway, subnet );
 #else
     WiFi.config( local_ip, gateway, subnet );
@@ -135,6 +135,14 @@ class WiFiStream : public Stream
   {
     return _client.status();
   }
+#elif ARDUINO_ARCH_ESP32
+  /**
+   * check if TCP connection is established
+   */
+  inline uint8_t connected()
+  {
+    return _client.connected();
+  }
 #endif
 
   /**
@@ -157,10 +165,10 @@ class WiFiStream : public Stream
 
     WiFi.begin(ssid);
     int result = WiFi.status();
-    return WiFi.status();
+    return result;
   }
 
-#ifndef ESP8266
+#if (!ESP8266) && (!ARDUINO_ARCH_ESP32)
   /**
    * initialize WiFi with WEP security and initiate client connection
    * if WiFi connection is already established

From 7a6dd824803e663c60cf9f12fb32374f2517bb3e Mon Sep 17 00:00:00 2001
From: "Jens B." <jnsbyr@gmail.com>
Date: Thu, 21 Dec 2023 20:30:22 +0100
Subject: [PATCH 02/12] WiFiStream: implement method status() instead of adding
 method connected()

---
 utility/WiFiStream.h | 24 ++++++++++++------------
 1 file changed, 12 insertions(+), 12 deletions(-)

diff --git a/utility/WiFiStream.h b/utility/WiFiStream.h
index b52e6d59..a3b35715 100644
--- a/utility/WiFiStream.h
+++ b/utility/WiFiStream.h
@@ -69,7 +69,7 @@ class WiFiStream : public Stream
  *           network configuration
  ******************************************************************************/
 
-#if (!ESP8266) && (!ARDUINO_ARCH_ESP32)
+#if !ESP8266 && !ESP32
   /**
    * configure a static local IP address without defining the local network
    * DHCP will be used as long as local IP address is not defined
@@ -90,7 +90,7 @@ class WiFiStream : public Stream
     _local_ip = local_ip;
     _subnet = subnet;
     _gateway = gateway;
-#if (!ESP8266) && (!ARDUINO_ARCH_ESP32)
+#if !ESP8266 && !ESP32
     WiFi.config( local_ip, IPAddress(0, 0, 0, 0), gateway, subnet );
 #else
     WiFi.config( local_ip, gateway, subnet );
@@ -115,7 +115,11 @@ class WiFiStream : public Stream
    */
   virtual bool maintain() = 0;
 
-#ifdef ESP8266
+#if ESP8266 || ESP32
+  enum ConnectionStatus { STATUS_CLOSED = 0,
+                          STATUS_ESTABLISHED = 4
+                        };
+
   /**
    * get status of TCP connection
    * @return status of TCP connection
@@ -133,15 +137,11 @@ class WiFiStream : public Stream
    */
   inline uint8_t status()
   {
+  #ifdef ESP8266
     return _client.status();
-  }
-#elif ARDUINO_ARCH_ESP32
-  /**
-   * check if TCP connection is established
-   */
-  inline uint8_t connected()
-  {
-    return _client.connected();
+  #elif ESP32
+    return _client.connected()? STATUS_ESTABLISHED : STATUS_CLOSED;
+  #endif
   }
 #endif
 
@@ -168,7 +168,7 @@ class WiFiStream : public Stream
     return result;
   }
 
-#if (!ESP8266) && (!ARDUINO_ARCH_ESP32)
+#if !ESP8266 && !ESP32
   /**
    * initialize WiFi with WEP security and initiate client connection
    * if WiFi connection is already established

From 9651a7c922b8e7009baab9af383860da6314965b Mon Sep 17 00:00:00 2001
From: "Jens B." <jnsbyr@gmail.com>
Date: Thu, 21 Dec 2023 20:32:20 +0100
Subject: [PATCH 03/12] StandardFirmataWiFi: include library ESP32Servo

---
 Boards.h                                          | 15 +++------------
 .../StandardFirmataWiFi/StandardFirmataWiFi.ino   |  6 +++---
 2 files changed, 6 insertions(+), 15 deletions(-)

diff --git a/Boards.h b/Boards.h
index 0404d562..67e06a7d 100644
--- a/Boards.h
+++ b/Boards.h
@@ -31,15 +31,6 @@
 // user intended by not including Servo.h
 #ifndef MAX_SERVOS
   #define MAX_SERVOS 0
-  class Servo
-  {
-    public:
-      uint8_t attach(int pin) { return 0; };
-      uint8_t attach(int pin, int min, int max) { return 0; };
-      void detach() {};
-      void write(int value) {};
-      bool attached() { return false; };
-  };
 #endif
 
 /*
@@ -1031,8 +1022,8 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 #define PIN_TO_SERVO(p)         (p)
 #define DEFAULT_PWM_RESOLUTION  10
 
-// XIAO ESP32C2
-// note: Firmata pin numbering schema is by ESP32 GPIO -> IS_XXX checks GPIO number
+// XIAO ESP32C3
+// note: Firmata pin numbering schema is by ESP32 GPIO -> IS_XXX checks GPIO number (Ax = Dx, Dx to GPIOy)
 #elif defined(ARDUINO_XIAO_ESP32C3)
 #define TOTAL_ANALOG_PINS       (A2 + 1)          // (max GPIOx + 1), there are 4 physical analog pins but only 3 are supported by ESP32 SDK 2.0.14 via ADC1
 #define TOTAL_PINS              NUM_DIGITAL_PINS  // (max GPIOx + 1), there are 11 physical pins
@@ -1051,7 +1042,7 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 #define PIN_TO_PWM(p)           127                                                 // @TODO ESP32 SDK does not support analogWrite()
 #define PIN_TO_SERVO(p)         127                                                 // @TODO ESP32 SDK does not support servos
 
-#define DEFAULT_PWM_RESOLUTION     8  // see esp32-hal-led.c, analog_resolution
+#define DEFAULT_PWM_RESOLUTION   8  // see esp32-hal-led.c, analog_resolution
 #define DEFAULT_ANALOG_RESOLUTION 12  // see esp32-hal-adc.h, analogSetWidth()
 
 // STM32 based boards
diff --git a/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino b/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino
index 0fc68481..b5947b3d 100644
--- a/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino
+++ b/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino
@@ -74,9 +74,9 @@
   - Arduino Mega: (D5, D7, D10, D50, D52, D53)
 */
 
-#ifndef ARDUINO_ARCH_ESP32
-  // NOTE: ESP32 SKD does not provide an implementation for class Servo
-  //       -> requires a skeleton implementation for Servo in Boards.h to be able to compile
+#ifdef ARDUINO_ARCH_ESP32
+  #include <ESP32Servo.h>
+#else
   #include <Servo.h>
 #endif
 #include <Wire.h>

From dde9fd913cb3d04fef1c11ec9eca265623055eda Mon Sep 17 00:00:00 2001
From: "Jens B." <jnsbyr@gmail.com>
Date: Thu, 21 Dec 2023 20:34:30 +0100
Subject: [PATCH 04/12] Boards.h: renamed DEFAULT_ANALOG_RESOLUTION to
 DEFAULT_ADC_RESOLUTION

---
 Boards.h                                             | 2 +-
 examples/StandardFirmataWiFi/StandardFirmataWiFi.ino | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/Boards.h b/Boards.h
index 67e06a7d..0089b2ee 100644
--- a/Boards.h
+++ b/Boards.h
@@ -1043,7 +1043,7 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 #define PIN_TO_SERVO(p)         127                                                 // @TODO ESP32 SDK does not support servos
 
 #define DEFAULT_PWM_RESOLUTION   8  // see esp32-hal-led.c, analog_resolution
-#define DEFAULT_ANALOG_RESOLUTION 12  // see esp32-hal-adc.h, analogSetWidth()
+#define DEFAULT_ADC_RESOLUTION  12  // see esp32-hal-adc.h, analogSetWidth()
 
 // STM32 based boards
 #elif defined(ARDUINO_ARCH_STM32)
diff --git a/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino b/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino
index b5947b3d..d8eb7520 100644
--- a/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino
+++ b/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino
@@ -740,8 +740,8 @@ void sysexCallback(byte command, byte argc, byte *argv)
         }
         if (IS_PIN_ANALOG(pin)) {
           Firmata.write(PIN_MODE_ANALOG);
-        #ifdef DEFAULT_ANALOG_RESOLUTION
-          Firmata.write(DEFAULT_ANALOG_RESOLUTION);
+        #ifdef DEFAULT_ADC_RESOLUTION
+          Firmata.write(DEFAULT_ADC_RESOLUTION);
         #else
           Firmata.write(10); // 10 = 10-bit resolution
         #endif

From ad79f15e3484ca739de2af4eb718cadd23681696 Mon Sep 17 00:00:00 2001
From: "Jens B." <jnsbyr@gmail.com>
Date: Thu, 21 Dec 2023 20:36:10 +0100
Subject: [PATCH 05/12] added board WT32-ETH01 (ESP32-S1)

---
 Boards.h | 25 +++++++++++++++++++++++++
 1 file changed, 25 insertions(+)

diff --git a/Boards.h b/Boards.h
index 0089b2ee..47461563 100644
--- a/Boards.h
+++ b/Boards.h
@@ -1045,6 +1045,31 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 #define DEFAULT_PWM_RESOLUTION   8  // see esp32-hal-led.c, analog_resolution
 #define DEFAULT_ADC_RESOLUTION  12  // see esp32-hal-adc.h, analogSetWidth()
 
+// WT32-ETH01 (ESP32-S1)
+// note: Firmata pin numbering schema is by ESP32 GPIO -> IS_XXX checks GPIO number (Ax = Dx = IOx = GPIOx)
+#elif defined(ARDUINO_WT32_ETH01)
+#define TOTAL_ANALOG_PINS       (IO17 + 1)        // (max GPIOx + 1), there are 10 physical analog pins
+#define TOTAL_PINS              NUM_DIGITAL_PINS  // (max GPIOx + 1), there are 15 physical pins, 3 of them are input only
+#define PIN_SERIAL1_RX          RX
+#define PIN_SERIAL1_TX          TX
+#define PIN_SERIAL2_RX          TXD
+#define PIN_SERIAL2_TX          RXD
+#define IS_PIN_DIGITAL(p)       (((p) <= IO5) || (p) == IO12 || (p) == IO14 || (p) == IO15 || (p) == IO17 || (p) == IO32 || (p) == IO33 || (p) == IO35 || (p) == IO39)
+#define IS_PIN_ANALOG(p)        (((p) <= IO5) || (p) == IO12 || (p) == IO14 || (p) == IO15 || (p) == IO17)
+#define IS_PIN_PWM(p)           0
+#define IS_PIN_SERVO(p)         (IS_PIN_DIGITAL(p) && MAX_SERVOS > 0)
+#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_INTERRUPT(p)     (digitalPinToInterrupt(p) > NOT_AN_INTERRUPT)
+#define IS_PIN_SERIAL(p)        ((p) == PIN_SERIAL1_RX || (p) == PIN_SERIAL1_TX || (p) == PIN_SERIAL2_RX || (p) == PIN_SERIAL2_TX)
+#define PIN_TO_DIGITAL(p)       (p)                                                 // FIRMATAx to GPIOy
+#define PIN_TO_ANALOG(p)        (p)                                                 // FIRMATAx to GPIOy
+#define PIN_TO_PWM(p)           127                                                 // @TODO ESP32 SDK does not support analogWrite()
+#define PIN_TO_SERVO(p)         127                                                 // @TODO ESP32 SDK does not support servos
+
+#define DEFAULT_PWM_RESOLUTION   8  // see esp32-hal-led.c, analog_resolution
+#define DEFAULT_ADC_RESOLUTION  12  // see esp32-hal-adc.h, analogSetWidth()
+
 // STM32 based boards
 #elif defined(ARDUINO_ARCH_STM32)
 #define TOTAL_ANALOG_PINS       NUM_ANALOG_INPUTS

From 4567db4855bc8e8f9b57247e9913d0c4873088da Mon Sep 17 00:00:00 2001
From: "Jens B." <jnsbyr@gmail.com>
Date: Sat, 23 Dec 2023 22:49:16 +0100
Subject: [PATCH 06/12] Boards.h: fixed PIN_TO_DIGITAL for board
 ARDUINO_XIAO_ESP32C3

---
 Boards.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/Boards.h b/Boards.h
index 47461563..060f7c10 100644
--- a/Boards.h
+++ b/Boards.h
@@ -1023,7 +1023,7 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 #define DEFAULT_PWM_RESOLUTION  10
 
 // XIAO ESP32C3
-// note: Firmata pin numbering schema is by ESP32 GPIO -> IS_XXX checks GPIO number (Ax = Dx, Dx to GPIOy)
+// note: Firmata pin numbering schema is by ESP32 GPIO -> IS_XXX checks GPIO number (Ax = Dx = GPIOx)
 #elif defined(ARDUINO_XIAO_ESP32C3)
 #define TOTAL_ANALOG_PINS       (A2 + 1)          // (max GPIOx + 1), there are 4 physical analog pins but only 3 are supported by ESP32 SDK 2.0.14 via ADC1
 #define TOTAL_PINS              NUM_DIGITAL_PINS  // (max GPIOx + 1), there are 11 physical pins
@@ -1037,7 +1037,7 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 #define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
 #define IS_PIN_INTERRUPT(p)     (digitalPinToInterrupt(p) > NOT_AN_INTERRUPT)
 #define IS_PIN_SERIAL(p)        ((p) == PIN_SERIAL_RX || (p) == PIN_SERIAL_TX)
-#define PIN_TO_DIGITAL(p)       ((p) < 6? D0 + (p) : ((p) < 8? D6 + 6 - (p) : (p))) // Dx to GPIOy
+#define PIN_TO_DIGITAL(p)       (p)                                                 // FIRMATAx to GPIOy
 #define PIN_TO_ANALOG(p)        (p)                                                 // FIRMATAx to GPIOy
 #define PIN_TO_PWM(p)           127                                                 // @TODO ESP32 SDK does not support analogWrite()
 #define PIN_TO_SERVO(p)         127                                                 // @TODO ESP32 SDK does not support servos

From 08325d145f53abe9bbf3fa8d24434f57da9de8b9 Mon Sep 17 00:00:00 2001
From: "Jens B." <jnsbyr@gmail.com>
Date: Sat, 23 Dec 2023 23:11:58 +0100
Subject: [PATCH 07/12] StandardFirmataWiFi: added error check and debug output
 for ESP32_WIFI

---
 examples/StandardFirmataWiFi/StandardFirmataWiFi.ino | 2 ++
 examples/StandardFirmataWiFi/wifiConfig.h            | 2 +-
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino b/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino
index d8eb7520..faf2bec9 100644
--- a/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino
+++ b/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino
@@ -941,6 +941,8 @@ void initTransport()
   DEBUG_PRINTLN( "using the legacy WiFi library." );
 #elif defined(ESP8266_WIFI)
   DEBUG_PRINTLN( "using the ESP8266 WiFi library." );
+#elif defined(ESP32_WIFI)
+  DEBUG_PRINTLN( "using the ESP32 WiFi library." );
 #elif defined(HUZZAH_WIFI)
   DEBUG_PRINTLN( "using the HUZZAH WiFi library." );
 #elif defined(WIFI_NINA)
diff --git a/examples/StandardFirmataWiFi/wifiConfig.h b/examples/StandardFirmataWiFi/wifiConfig.h
index 4a4955bd..fe7f0979 100644
--- a/examples/StandardFirmataWiFi/wifiConfig.h
+++ b/examples/StandardFirmataWiFi/wifiConfig.h
@@ -262,7 +262,7 @@ char wep_key[] = "your_wep_key";
 #error "you must define a wifi security type in wifiConfig.h."
 #endif  //WIFI_* security define check
 
-#if (defined(ESP8266_WIFI) && !(defined(WIFI_NO_SECURITY) || (defined(WIFI_WPA_SECURITY))))
+#if ((defined(ESP8266_WIFI) || defined(ESP32_WIFI)) && !(defined(WIFI_NO_SECURITY) || (defined(WIFI_WPA_SECURITY))))
 #error "you must choose between WIFI_NO_SECURITY and WIFI_WPA_SECURITY"
 #endif
 

From af2e9aa2ce87423bd64ccdbad14eae05aae36182 Mon Sep 17 00:00:00 2001
From: "Jens B." <jnsbyr@gmail.com>
Date: Sun, 24 Dec 2023 00:27:58 +0100
Subject: [PATCH 08/12] Boards.h: enabled PIN_TO_SERVO for ARDUINO_XIAO_ESP32C3
 and ARDUINO_WT32_ETH01, fixed TOTAL_ANALOG_PINS, IS_PIN_DIGITAL IS_PIN_ANALOG
 for ARDUINO_WT32_ETH01

---
 Boards.h | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/Boards.h b/Boards.h
index 060f7c10..f0047e8f 100644
--- a/Boards.h
+++ b/Boards.h
@@ -1040,7 +1040,7 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 #define PIN_TO_DIGITAL(p)       (p)                                                 // FIRMATAx to GPIOy
 #define PIN_TO_ANALOG(p)        (p)                                                 // FIRMATAx to GPIOy
 #define PIN_TO_PWM(p)           127                                                 // @TODO ESP32 SDK does not support analogWrite()
-#define PIN_TO_SERVO(p)         127                                                 // @TODO ESP32 SDK does not support servos
+#define PIN_TO_SERVO(p)         ((p) - D0)
 
 #define DEFAULT_PWM_RESOLUTION   8  // see esp32-hal-led.c, analog_resolution
 #define DEFAULT_ADC_RESOLUTION  12  // see esp32-hal-adc.h, analogSetWidth()
@@ -1048,14 +1048,14 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 // WT32-ETH01 (ESP32-S1)
 // note: Firmata pin numbering schema is by ESP32 GPIO -> IS_XXX checks GPIO number (Ax = Dx = IOx = GPIOx)
 #elif defined(ARDUINO_WT32_ETH01)
-#define TOTAL_ANALOG_PINS       (IO17 + 1)        // (max GPIOx + 1), there are 10 physical analog pins
+#define TOTAL_ANALOG_PINS       NUM_ANALOG_INPUTS // (max GPIOx + 1), there are 9 physical analog pins
 #define TOTAL_PINS              NUM_DIGITAL_PINS  // (max GPIOx + 1), there are 15 physical pins, 3 of them are input only
 #define PIN_SERIAL1_RX          RX
 #define PIN_SERIAL1_TX          TX
 #define PIN_SERIAL2_RX          TXD
 #define PIN_SERIAL2_TX          RXD
-#define IS_PIN_DIGITAL(p)       (((p) <= IO5) || (p) == IO12 || (p) == IO14 || (p) == IO15 || (p) == IO17 || (p) == IO32 || (p) == IO33 || (p) == IO35 || (p) == IO39)
-#define IS_PIN_ANALOG(p)        (((p) <= IO5) || (p) == IO12 || (p) == IO14 || (p) == IO15 || (p) == IO17)
+#define IS_PIN_DIGITAL(p)       (((p) <= IO5) || (p) == IO12 || (p) == IO14 || (p) == IO15 || (p) == IO17 || (p) == IO32 || (p) == IO33 || (p) == IO35 || (p) == IO36 || (p) == IO39)
+#define IS_PIN_ANALOG(p)        (((p) <= IO5) || (p) == IO12 || (p) == IO14 || (p) == IO15)
 #define IS_PIN_PWM(p)           0
 #define IS_PIN_SERVO(p)         (IS_PIN_DIGITAL(p) && MAX_SERVOS > 0)
 #define IS_PIN_I2C(p)           ((p) == SDA || (p) == SCL)
@@ -1065,7 +1065,7 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 #define PIN_TO_DIGITAL(p)       (p)                                                 // FIRMATAx to GPIOy
 #define PIN_TO_ANALOG(p)        (p)                                                 // FIRMATAx to GPIOy
 #define PIN_TO_PWM(p)           127                                                 // @TODO ESP32 SDK does not support analogWrite()
-#define PIN_TO_SERVO(p)         127                                                 // @TODO ESP32 SDK does not support servos
+#define PIN_TO_SERVO(p)         (p)
 
 #define DEFAULT_PWM_RESOLUTION   8  // see esp32-hal-led.c, analog_resolution
 #define DEFAULT_ADC_RESOLUTION  12  // see esp32-hal-adc.h, analogSetWidth()

From 90ac082c2480a53c3dd21b93bb368ab374831d46 Mon Sep 17 00:00:00 2001
From: "Jens B." <jnsbyr@gmail.com>
Date: Sun, 24 Dec 2023 00:29:37 +0100
Subject: [PATCH 09/12] Boards.h: added generic board defintion for ESP32

---
 Boards.h | 23 +++++++++++++++++++++++
 1 file changed, 23 insertions(+)

diff --git a/Boards.h b/Boards.h
index f0047e8f..ed7560cb 100644
--- a/Boards.h
+++ b/Boards.h
@@ -1070,6 +1070,29 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 #define DEFAULT_PWM_RESOLUTION   8  // see esp32-hal-led.c, analog_resolution
 #define DEFAULT_ADC_RESOLUTION  12  // see esp32-hal-adc.h, analogSetWidth()
 
+// generic ESP32
+// note: Firmata pin numbering schema is by ESP32 GPIO -> IS_XXX checks GPIO number (Ax = Dx = IOx = GPIOx)
+#elif defined(ARDUINO_ARCH_ESP32)
+#define TOTAL_ANALOG_PINS       NUM_ANALOG_INPUTS // (max GPIOx + 1), there are 4 physical analog pins but only 3 are supported by ESP32 SDK 2.0.14 via ADC1
+#define TOTAL_PINS              NUM_DIGITAL_PINS  // (max GPIOx + 1), there are 11 physical pins
+#define PIN_SERIAL_RX           RX
+#define PIN_SERIAL_TX           TX
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < NUM_DIGITAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 0 && (p) < NUM_ANALOG_INPUTS)
+#define IS_PIN_PWM(p)           0
+#define IS_PIN_SERVO(p)         (IS_PIN_DIGITAL(p) && MAX_SERVOS > 0)
+#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_INTERRUPT(p)     (digitalPinToInterrupt(p) > NOT_AN_INTERRUPT)
+#define IS_PIN_SERIAL(p)        ((p) == PIN_SERIAL_RX || (p) == PIN_SERIAL_TX)
+#define PIN_TO_DIGITAL(p)       (p)                                                 // FIRMATAx to GPIOy
+#define PIN_TO_ANALOG(p)        (p)                                                 // FIRMATAx to GPIOy
+#define PIN_TO_PWM(p)           127                                                 // @TODO ESP32 SDK does not support analogWrite()
+#define PIN_TO_SERVO(p)         (p)
+
+#define DEFAULT_PWM_RESOLUTION   8  // see esp32-hal-led.c, analog_resolution
+#define DEFAULT_ADC_RESOLUTION  12  // see esp32-hal-adc.h, analogSetWidth()
+
 // STM32 based boards
 #elif defined(ARDUINO_ARCH_STM32)
 #define TOTAL_ANALOG_PINS       NUM_ANALOG_INPUTS

From 44a878895a0263f7d4c349e0874ad10c9c58dd1b Mon Sep 17 00:00:00 2001
From: "Jens B." <jnsbyr@gmail.com>
Date: Wed, 3 Jan 2024 19:50:59 +0100
Subject: [PATCH 10/12] Boards.h: exclude SPI flash pins for generic ESP32
 board

---
 Boards.h | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/Boards.h b/Boards.h
index ed7560cb..58fa574e 100644
--- a/Boards.h
+++ b/Boards.h
@@ -1073,11 +1073,19 @@ writePort(port, value, bitmask):  Write an 8 bit port.
 // generic ESP32
 // note: Firmata pin numbering schema is by ESP32 GPIO -> IS_XXX checks GPIO number (Ax = Dx = IOx = GPIOx)
 #elif defined(ARDUINO_ARCH_ESP32)
-#define TOTAL_ANALOG_PINS       NUM_ANALOG_INPUTS // (max GPIOx + 1), there are 4 physical analog pins but only 3 are supported by ESP32 SDK 2.0.14 via ADC1
-#define TOTAL_PINS              NUM_DIGITAL_PINS  // (max GPIOx + 1), there are 11 physical pins
+#define TOTAL_ANALOG_PINS       NUM_ANALOG_INPUTS // (max GPIOx + 1)
+#define TOTAL_PINS              NUM_DIGITAL_PINS  // (max GPIOx + 1)
 #define PIN_SERIAL_RX           RX
 #define PIN_SERIAL_TX           TX
-#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < NUM_DIGITAL_PINS)
+#if defined(CONFIG_IDF_TARGET_ESP32)
+  #define IS_PIN_DIGITAL(p)       (((p) >= 0 && (p) < NUM_DIGITAL_PINS) && !((p) >= 6 && (p) <= 11))  // exclude SPI flash pins
+#elif defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3)
+  #define IS_PIN_DIGITAL(p)       (((p) >= 0 && (p) < NUM_DIGITAL_PINS) && !((p) >= 26 && (p) <= 32)) // exclude SPI flash pins
+#elif defined(CONFIG_IDF_TARGET_ESP32C3)
+  #define IS_PIN_DIGITAL(p)       (((p) >= 0 && (p) < NUM_DIGITAL_PINS) && !((p) >= 11 && (p) <= 17)) // exclude SPI flash pins
+#else
+  #error "Please edit Boards.h with a hardware abstraction for this board"
+#endif
 #define IS_PIN_ANALOG(p)        ((p) >= 0 && (p) < NUM_ANALOG_INPUTS)
 #define IS_PIN_PWM(p)           0
 #define IS_PIN_SERVO(p)         (IS_PIN_DIGITAL(p) && MAX_SERVOS > 0)

From b3a78d5a3e776557de803089df7ba67614696978 Mon Sep 17 00:00:00 2001
From: "Jens B." <jnsbyr@gmail.com>
Date: Wed, 3 Jan 2024 19:54:16 +0100
Subject: [PATCH 11/12] synced changes in StandardFirmataWiFi to all examples

---
 examples/StandardFirmata/StandardFirmata.ino  | 54 +++++++++++--------
 .../StandardFirmataBLE/StandardFirmataBLE.ino | 50 ++++++++++-------
 .../StandardFirmataEthernet.ino               | 49 ++++++++++-------
 .../StandardFirmataPlus.ino                   | 54 +++++++++++--------
 .../StandardFirmataWiFi.ino                   |  7 +--
 5 files changed, 131 insertions(+), 83 deletions(-)

diff --git a/examples/StandardFirmata/StandardFirmata.ino b/examples/StandardFirmata/StandardFirmata.ino
index 0e05a812..b7a6a833 100755
--- a/examples/StandardFirmata/StandardFirmata.ino
+++ b/examples/StandardFirmata/StandardFirmata.ino
@@ -12,6 +12,7 @@
   Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
   Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
   Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+  Copyright (C) 2023 Jens B. 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
@@ -20,10 +21,14 @@
 
   See file LICENSE.txt for further informations on licensing terms.
 
-  Last updated August 17th, 2017
+  Last updated December 17th, 2023
 */
 
-#include <Servo.h>
+#ifdef ARDUINO_ARCH_ESP32
+  #include <ESP32Servo.h>
+#else
+  #include <Servo.h>
+#endif
 #include <Wire.h>
 #include <Firmata.h>
 
@@ -52,7 +57,7 @@ SerialFirmata serialFeature;
 #endif
 
 /* analog inputs */
-int analogInputsToReport = 0; // bitwise array to store pin reporting
+int analogInputsToReport = 0;       // bitwise array to store pin reporting
 
 /* digital input ports */
 byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
@@ -64,7 +69,7 @@ byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything
 /* 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)
+unsigned int samplingInterval = 19; // how often to sample analog inputs and i2c (in ms)
 
 /* i2c data */
 struct i2c_device_info {
@@ -74,7 +79,7 @@ struct i2c_device_info {
   byte stopTX;
 };
 
-/* for i2c read continuous more */
+/* for i2c read continuous mode */
 i2c_device_info query[I2C_MAX_QUERIES];
 
 byte i2cRxData[64];
@@ -151,8 +156,10 @@ void detachServo(byte pin)
   } 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];
+    if (detachedServoCount < MAX_SERVOS) {
+      detachedServos[detachedServoCount] = servoPinMap[pin];
+      detachedServoCount++;
+    }
   }
 
   servoPinMap[pin] = 255;
@@ -277,10 +284,11 @@ void setPinModeCallback(byte pin, int mode)
     }
   }
   if (IS_PIN_ANALOG(pin)) {
-    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+    // turn on/off reporting
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0);
   }
   if (IS_PIN_DIGITAL(pin)) {
-    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+    if (mode == PIN_MODE_INPUT || mode == PIN_MODE_PULLUP) {
       portConfigInputs[pin / 8] |= (1 << (pin & 7));
     } else {
       portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
@@ -300,14 +308,14 @@ void setPinModeCallback(byte pin, int mode)
         Firmata.setPinMode(pin, PIN_MODE_ANALOG);
       }
       break;
-    case INPUT:
+    case PIN_MODE_INPUT:
       if (IS_PIN_DIGITAL(pin)) {
         pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
 #if ARDUINO <= 100
         // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
         digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
 #endif
-        Firmata.setPinMode(pin, INPUT);
+        Firmata.setPinMode(pin, PIN_MODE_INPUT);
       }
       break;
     case PIN_MODE_PULLUP:
@@ -317,14 +325,14 @@ void setPinModeCallback(byte pin, int mode)
         Firmata.setPinState(pin, 1);
       }
       break;
-    case OUTPUT:
+    case PIN_MODE_OUTPUT:
       if (IS_PIN_DIGITAL(pin)) {
         if (Firmata.getPinMode(pin) == PIN_MODE_PWM) {
           // Disable PWM if pin mode was previously set to PWM.
           digitalWrite(PIN_TO_DIGITAL(pin), LOW);
         }
         pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
-        Firmata.setPinMode(pin, OUTPUT);
+        Firmata.setPinMode(pin, PIN_MODE_OUTPUT);
       }
       break;
     case PIN_MODE_PWM:
@@ -371,7 +379,7 @@ void setPinModeCallback(byte pin, int mode)
 void setPinValueCallback(byte pin, int value)
 {
   if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
-    if (Firmata.getPinMode(pin) == OUTPUT) {
+    if (Firmata.getPinMode(pin) == PIN_MODE_OUTPUT) {
       Firmata.setPinState(pin, value);
       digitalWrite(PIN_TO_DIGITAL(pin), value);
     }
@@ -408,11 +416,11 @@ void digitalWriteCallback(byte port, int value)
       // do not disturb non-digital pins (eg, Rx & Tx)
       if (IS_PIN_DIGITAL(pin)) {
         // do not touch pins in PWM, ANALOG, SERVO or other modes
-        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+        if (Firmata.getPinMode(pin) == PIN_MODE_OUTPUT || Firmata.getPinMode(pin) == PIN_MODE_INPUT) {
           pinValue = ((byte)value & mask) ? 1 : 0;
-          if (Firmata.getPinMode(pin) == OUTPUT) {
+          if (Firmata.getPinMode(pin) == PIN_MODE_OUTPUT) {
             pinWriteMask |= mask;
-          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+          } else if (Firmata.getPinMode(pin) == PIN_MODE_INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
             // only handle INPUT here for backwards compatibility
 #if ARDUINO > 100
             pinMode(pin, INPUT_PULLUP);
@@ -635,22 +643,26 @@ void sysexCallback(byte command, byte argc, byte *argv)
       Firmata.write(CAPABILITY_RESPONSE);
       for (byte pin = 0; pin < TOTAL_PINS; pin++) {
         if (IS_PIN_DIGITAL(pin)) {
-          Firmata.write((byte)INPUT);
+          Firmata.write((byte)PIN_MODE_INPUT);
           Firmata.write(1);
           Firmata.write((byte)PIN_MODE_PULLUP);
           Firmata.write(1);
-          Firmata.write((byte)OUTPUT);
+          Firmata.write((byte)PIN_MODE_OUTPUT);
           Firmata.write(1);
         }
         if (IS_PIN_ANALOG(pin)) {
           Firmata.write(PIN_MODE_ANALOG);
+        #ifdef DEFAULT_ADC_RESOLUTION
+          Firmata.write(DEFAULT_ADC_RESOLUTION);
+        #else
           Firmata.write(10); // 10 = 10-bit resolution
+        #endif
         }
         if (IS_PIN_PWM(pin)) {
           Firmata.write(PIN_MODE_PWM);
           Firmata.write(DEFAULT_PWM_RESOLUTION);
         }
-        if (IS_PIN_DIGITAL(pin)) {
+        if (IS_PIN_SERVO(pin)) {
           Firmata.write(PIN_MODE_SERVO);
           Firmata.write(14);
         }
@@ -730,7 +742,7 @@ void systemResetCallback()
       setPinModeCallback(i, PIN_MODE_ANALOG);
     } else if (IS_PIN_DIGITAL(i)) {
       // sets the output to 0, configures portConfigInputs
-      setPinModeCallback(i, OUTPUT);
+      setPinModeCallback(i, PIN_MODE_OUTPUT);
     }
 
     servoPinMap[i] = 255;
diff --git a/examples/StandardFirmataBLE/StandardFirmataBLE.ino b/examples/StandardFirmataBLE/StandardFirmataBLE.ino
index ba80e8d8..ddeac535 100755
--- a/examples/StandardFirmataBLE/StandardFirmataBLE.ino
+++ b/examples/StandardFirmataBLE/StandardFirmataBLE.ino
@@ -12,6 +12,7 @@
   Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
   Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
   Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+  Copyright (C) 2023 Jens B. 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
@@ -20,10 +21,14 @@
 
   See file LICENSE.txt for further informations on licensing terms.
 
-  Last updated April 15th, 2018
+  Last updated December 17th, 2023
 */
 
-#include <Servo.h>
+#ifdef ARDUINO_ARCH_ESP32
+  #include <ESP32Servo.h>
+#else
+  #include <Servo.h>
+#endif
 #include <Wire.h>
 #include <Firmata.h>
 
@@ -65,7 +70,7 @@ SerialFirmata serialFeature;
 #endif
 
 /* analog inputs */
-int analogInputsToReport = 0; // bitwise array to store pin reporting
+int analogInputsToReport = 0;       // bitwise array to store pin reporting
 
 /* digital input ports */
 byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
@@ -77,7 +82,7 @@ byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything
 /* 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)
+unsigned int samplingInterval = 19; // how often to sample analog inputs and i2c (in ms)
 
 /* i2c data */
 struct i2c_device_info {
@@ -87,7 +92,7 @@ struct i2c_device_info {
   byte stopTX;
 };
 
-/* for i2c read continuous more */
+/* for i2c read continuous mode */
 i2c_device_info query[I2C_MAX_QUERIES];
 
 byte i2cRxData[64];
@@ -164,8 +169,10 @@ void detachServo(byte pin)
   } 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];
+    if (detachedServoCount < MAX_SERVOS) {
+      detachedServos[detachedServoCount] = servoPinMap[pin];
+      detachedServoCount++;
+    }
   }
 
   servoPinMap[pin] = 255;
@@ -290,10 +297,11 @@ void setPinModeCallback(byte pin, int mode)
     }
   }
   if (IS_PIN_ANALOG(pin)) {
-    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+    // turn on/off reporting
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0);
   }
   if (IS_PIN_DIGITAL(pin)) {
-    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+    if (mode == PIN_MODE_INPUT || mode == PIN_MODE_PULLUP) {
       portConfigInputs[pin / 8] |= (1 << (pin & 7));
     } else {
       portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
@@ -313,14 +321,14 @@ void setPinModeCallback(byte pin, int mode)
         Firmata.setPinMode(pin, PIN_MODE_ANALOG);
       }
       break;
-    case INPUT:
+    case PIN_MODE_INPUT:
       if (IS_PIN_DIGITAL(pin)) {
         pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
 #if ARDUINO <= 100
         // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
         digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
 #endif
-        Firmata.setPinMode(pin, INPUT);
+        Firmata.setPinMode(pin, PIN_MODE_INPUT);
       }
       break;
     case PIN_MODE_PULLUP:
@@ -330,14 +338,14 @@ void setPinModeCallback(byte pin, int mode)
         Firmata.setPinState(pin, 1);
       }
       break;
-    case OUTPUT:
+    case PIN_MODE_OUTPUT:
       if (IS_PIN_DIGITAL(pin)) {
         if (Firmata.getPinMode(pin) == PIN_MODE_PWM) {
           // Disable PWM if pin mode was previously set to PWM.
           digitalWrite(PIN_TO_DIGITAL(pin), LOW);
         }
         pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
-        Firmata.setPinMode(pin, OUTPUT);
+        Firmata.setPinMode(pin, PIN_MODE_OUTPUT);
       }
       break;
     case PIN_MODE_PWM:
@@ -384,7 +392,7 @@ void setPinModeCallback(byte pin, int mode)
 void setPinValueCallback(byte pin, int value)
 {
   if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
-    if (Firmata.getPinMode(pin) == OUTPUT) {
+    if (Firmata.getPinMode(pin) == PIN_MODE_OUTPUT) {
       Firmata.setPinState(pin, value);
       digitalWrite(PIN_TO_DIGITAL(pin), value);
     }
@@ -421,11 +429,11 @@ void digitalWriteCallback(byte port, int value)
       // do not disturb non-digital pins (eg, Rx & Tx)
       if (IS_PIN_DIGITAL(pin)) {
         // do not touch pins in PWM, ANALOG, SERVO or other modes
-        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+        if (Firmata.getPinMode(pin) == PIN_MODE_OUTPUT || Firmata.getPinMode(pin) == PIN_MODE_INPUT) {
           pinValue = ((byte)value & mask) ? 1 : 0;
-          if (Firmata.getPinMode(pin) == OUTPUT) {
+          if (Firmata.getPinMode(pin) == PIN_MODE_OUTPUT) {
             pinWriteMask |= mask;
-          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+          } else if (Firmata.getPinMode(pin) == PIN_MODE_INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
             // only handle INPUT here for backwards compatibility
 #if ARDUINO > 100
             pinMode(pin, INPUT_PULLUP);
@@ -657,11 +665,15 @@ void sysexCallback(byte command, byte argc, byte *argv)
         }
         if (IS_PIN_ANALOG(pin)) {
           Firmata.write(PIN_MODE_ANALOG);
+        #ifdef DEFAULT_ADC_RESOLUTION
+          Firmata.write(DEFAULT_ADC_RESOLUTION);
+        #else
           Firmata.write(10); // 10 = 10-bit resolution
+        #endif
         }
         if (IS_PIN_PWM(pin)) {
           Firmata.write(PIN_MODE_PWM);
-          Firmata.write(8); // 8 = 8-bit resolution
+          Firmata.write(DEFAULT_PWM_RESOLUTION);
         }
         if (IS_PIN_DIGITAL(pin)) {
           Firmata.write(PIN_MODE_SERVO);
@@ -740,7 +752,7 @@ void systemResetCallback()
       setPinModeCallback(i, PIN_MODE_ANALOG);
     } else if (IS_PIN_DIGITAL(i)) {
       // sets the output to 0, configures portConfigInputs
-      setPinModeCallback(i, OUTPUT);
+      setPinModeCallback(i, PIN_MODE_OUTPUT);
     }
 
     servoPinMap[i] = 255;
diff --git a/examples/StandardFirmataEthernet/StandardFirmataEthernet.ino b/examples/StandardFirmataEthernet/StandardFirmataEthernet.ino
index 6795c215..d40e279b 100644
--- a/examples/StandardFirmataEthernet/StandardFirmataEthernet.ino
+++ b/examples/StandardFirmataEthernet/StandardFirmataEthernet.ino
@@ -12,6 +12,7 @@
   Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
   Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
   Copyright (C) 2009-2017 Jeff Hoefs.  All rights reserved.
+  Copyright (C) 2023 Jens B. 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
@@ -20,7 +21,7 @@
 
   See file LICENSE.txt for further informations on licensing terms.
 
-  Last updated March 10th, 2020
+  Last updated December 17th, 2023
 */
 
 /*
@@ -53,7 +54,11 @@
   - D4, D10, D11, D12, D13
 */
 
-#include <Servo.h>
+#ifdef ARDUINO_ARCH_ESP32
+  #include <ESP32Servo.h>
+#else
+  #include <Servo.h>
+#endif
 #include <Wire.h>
 #include <Firmata.h>
 
@@ -131,7 +136,7 @@ SerialFirmata serialFeature;
 #endif
 
 /* analog inputs */
-int analogInputsToReport = 0;      // bitwise array to store pin reporting
+int analogInputsToReport = 0;       // bitwise array to store pin reporting
 
 /* digital input ports */
 byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
@@ -143,7 +148,7 @@ byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything
 /* 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)
+unsigned int samplingInterval = 19; // how often to sample analog inputs and i2c (in ms)
 
 /* i2c data */
 struct i2c_device_info {
@@ -230,8 +235,10 @@ void detachServo(byte pin)
   } 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];
+    if (detachedServoCount < MAX_SERVOS) {
+      detachedServos[detachedServoCount] = servoPinMap[pin];
+      detachedServoCount++;
+    }
   }
 
   servoPinMap[pin] = 255;
@@ -360,7 +367,7 @@ void setPinModeCallback(byte pin, int mode)
     reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0);
   }
   if (IS_PIN_DIGITAL(pin)) {
-    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+    if (mode == PIN_MODE_INPUT || mode == PIN_MODE_PULLUP) {
       portConfigInputs[pin / 8] |= (1 << (pin & 7));
     } else {
       portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
@@ -380,14 +387,14 @@ void setPinModeCallback(byte pin, int mode)
         Firmata.setPinMode(pin, PIN_MODE_ANALOG);
       }
       break;
-    case INPUT:
+    case PIN_MODE_INPUT:
       if (IS_PIN_DIGITAL(pin)) {
         pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
 #if ARDUINO <= 100
         // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
         digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
 #endif
-        Firmata.setPinMode(pin, INPUT);
+        Firmata.setPinMode(pin, PIN_MODE_INPUT);
       }
       break;
     case PIN_MODE_PULLUP:
@@ -397,14 +404,14 @@ void setPinModeCallback(byte pin, int mode)
         Firmata.setPinState(pin, 1);
       }
       break;
-    case OUTPUT:
+    case PIN_MODE_OUTPUT:
       if (IS_PIN_DIGITAL(pin)) {
         if (Firmata.getPinMode(pin) == PIN_MODE_PWM) {
           // Disable PWM if pin mode was previously set to PWM.
           digitalWrite(PIN_TO_DIGITAL(pin), LOW);
         }
         pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
-        Firmata.setPinMode(pin, OUTPUT);
+        Firmata.setPinMode(pin, PIN_MODE_OUTPUT);
       }
       break;
     case PIN_MODE_PWM:
@@ -451,7 +458,7 @@ void setPinModeCallback(byte pin, int mode)
 void setPinValueCallback(byte pin, int value)
 {
   if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
-    if (Firmata.getPinMode(pin) == OUTPUT) {
+    if (Firmata.getPinMode(pin) == PIN_MODE_OUTPUT) {
       Firmata.setPinState(pin, value);
       digitalWrite(PIN_TO_DIGITAL(pin), value);
     }
@@ -488,11 +495,11 @@ void digitalWriteCallback(byte port, int value)
       // do not disturb non-digital pins (eg, Rx & Tx)
       if (IS_PIN_DIGITAL(pin)) {
         // do not touch pins in PWM, ANALOG, SERVO or other modes
-        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+        if (Firmata.getPinMode(pin) == PIN_MODE_OUTPUT || Firmata.getPinMode(pin) == PIN_MODE_INPUT) {
           pinValue = ((byte)value & mask) ? 1 : 0;
-          if (Firmata.getPinMode(pin) == OUTPUT) {
+          if (Firmata.getPinMode(pin) == PIN_MODE_OUTPUT) {
             pinWriteMask |= mask;
-          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+          } else if (Firmata.getPinMode(pin) == PIN_MODE_INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
             // only handle INPUT here for backwards compatibility
 #if ARDUINO > 100
             pinMode(pin, INPUT_PULLUP);
@@ -715,22 +722,26 @@ void sysexCallback(byte command, byte argc, byte *argv)
       Firmata.write(CAPABILITY_RESPONSE);
       for (byte pin = 0; pin < TOTAL_PINS; pin++) {
         if (IS_PIN_DIGITAL(pin)) {
-          Firmata.write((byte)INPUT);
+          Firmata.write((byte)PIN_MODE_INPUT);
           Firmata.write(1);
           Firmata.write((byte)PIN_MODE_PULLUP);
           Firmata.write(1);
-          Firmata.write((byte)OUTPUT);
+          Firmata.write((byte)PIN_MODE_OUTPUT);
           Firmata.write(1);
         }
         if (IS_PIN_ANALOG(pin)) {
           Firmata.write(PIN_MODE_ANALOG);
+        #ifdef DEFAULT_ADC_RESOLUTION
+          Firmata.write(DEFAULT_ADC_RESOLUTION);
+        #else
           Firmata.write(10); // 10 = 10-bit resolution
+        #endif
         }
         if (IS_PIN_PWM(pin)) {
           Firmata.write(PIN_MODE_PWM);
           Firmata.write(DEFAULT_PWM_RESOLUTION);
         }
-        if (IS_PIN_DIGITAL(pin)) {
+        if (IS_PIN_SERVO(pin)) {
           Firmata.write(PIN_MODE_SERVO);
           Firmata.write(14);
         }
@@ -810,7 +821,7 @@ void systemResetCallback()
       setPinModeCallback(i, PIN_MODE_ANALOG);
     } else if (IS_PIN_DIGITAL(i)) {
       // sets the output to 0, configures portConfigInputs
-      setPinModeCallback(i, OUTPUT);
+      setPinModeCallback(i, PIN_MODE_OUTPUT);
     }
 
     servoPinMap[i] = 255;
diff --git a/examples/StandardFirmataPlus/StandardFirmataPlus.ino b/examples/StandardFirmataPlus/StandardFirmataPlus.ino
index caece95c..0a7092c4 100644
--- a/examples/StandardFirmataPlus/StandardFirmataPlus.ino
+++ b/examples/StandardFirmataPlus/StandardFirmataPlus.ino
@@ -12,6 +12,7 @@
   Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
   Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
   Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+  Copyright (C) 2023 Jens B. 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
@@ -20,7 +21,7 @@
 
   See file LICENSE.txt for further informations on licensing terms.
 
-  Last updated August 17th, 2017
+  Last updated December 17th, 2023
 */
 
 /*
@@ -44,7 +45,11 @@
   may not as new features are added.
 */
 
-#include <Servo.h>
+#ifdef ARDUINO_ARCH_ESP32
+  #include <ESP32Servo.h>
+#else
+  #include <Servo.h>
+#endif
 #include <Wire.h>
 #include <Firmata.h>
 
@@ -77,7 +82,7 @@ SerialFirmata serialFeature;
 #endif
 
 /* analog inputs */
-int analogInputsToReport = 0; // bitwise array to store pin reporting
+int analogInputsToReport = 0;       // bitwise array to store pin reporting
 
 /* digital input ports */
 byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
@@ -89,7 +94,7 @@ byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything
 /* 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)
+unsigned int samplingInterval = 19; // how often to sample analog inputs and i2c (in ms)
 
 /* i2c data */
 struct i2c_device_info {
@@ -99,7 +104,7 @@ struct i2c_device_info {
   byte stopTX;
 };
 
-/* for i2c read continuous more */
+/* for i2c read continuous mode */
 i2c_device_info query[I2C_MAX_QUERIES];
 
 byte i2cRxData[64];
@@ -176,8 +181,10 @@ void detachServo(byte pin)
   } 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];
+    if (detachedServoCount < MAX_SERVOS) {
+      detachedServos[detachedServoCount] = servoPinMap[pin];
+      detachedServoCount++;
+    }
   }
 
   servoPinMap[pin] = 255;
@@ -302,10 +309,11 @@ void setPinModeCallback(byte pin, int mode)
     }
   }
   if (IS_PIN_ANALOG(pin)) {
-    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+    // turn on/off reporting
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0);
   }
   if (IS_PIN_DIGITAL(pin)) {
-    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+    if (mode == PIN_MODE_INPUT || mode == PIN_MODE_PULLUP) {
       portConfigInputs[pin / 8] |= (1 << (pin & 7));
     } else {
       portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
@@ -325,14 +333,14 @@ void setPinModeCallback(byte pin, int mode)
         Firmata.setPinMode(pin, PIN_MODE_ANALOG);
       }
       break;
-    case INPUT:
+    case PIN_MODE_INPUT:
       if (IS_PIN_DIGITAL(pin)) {
         pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
 #if ARDUINO <= 100
         // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
         digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
 #endif
-        Firmata.setPinMode(pin, INPUT);
+        Firmata.setPinMode(pin, PIN_MODE_INPUT);
       }
       break;
     case PIN_MODE_PULLUP:
@@ -342,14 +350,14 @@ void setPinModeCallback(byte pin, int mode)
         Firmata.setPinState(pin, 1);
       }
       break;
-    case OUTPUT:
+    case PIN_MODE_OUTPUT:
       if (IS_PIN_DIGITAL(pin)) {
         if (Firmata.getPinMode(pin) == PIN_MODE_PWM) {
           // Disable PWM if pin mode was previously set to PWM.
           digitalWrite(PIN_TO_DIGITAL(pin), LOW);
         }
         pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
-        Firmata.setPinMode(pin, OUTPUT);
+        Firmata.setPinMode(pin, PIN_MODE_OUTPUT);
       }
       break;
     case PIN_MODE_PWM:
@@ -396,7 +404,7 @@ void setPinModeCallback(byte pin, int mode)
 void setPinValueCallback(byte pin, int value)
 {
   if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
-    if (Firmata.getPinMode(pin) == OUTPUT) {
+    if (Firmata.getPinMode(pin) == PIN_MODE_OUTPUT) {
       Firmata.setPinState(pin, value);
       digitalWrite(PIN_TO_DIGITAL(pin), value);
     }
@@ -433,11 +441,11 @@ void digitalWriteCallback(byte port, int value)
       // do not disturb non-digital pins (eg, Rx & Tx)
       if (IS_PIN_DIGITAL(pin)) {
         // do not touch pins in PWM, ANALOG, SERVO or other modes
-        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+        if (Firmata.getPinMode(pin) == PIN_MODE_OUTPUT || Firmata.getPinMode(pin) == PIN_MODE_INPUT) {
           pinValue = ((byte)value & mask) ? 1 : 0;
-          if (Firmata.getPinMode(pin) == OUTPUT) {
+          if (Firmata.getPinMode(pin) == PIN_MODE_OUTPUT) {
             pinWriteMask |= mask;
-          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+          } else if (Firmata.getPinMode(pin) == PIN_MODE_INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
             // only handle INPUT here for backwards compatibility
 #if ARDUINO > 100
             pinMode(pin, INPUT_PULLUP);
@@ -660,22 +668,26 @@ void sysexCallback(byte command, byte argc, byte *argv)
       Firmata.write(CAPABILITY_RESPONSE);
       for (byte pin = 0; pin < TOTAL_PINS; pin++) {
         if (IS_PIN_DIGITAL(pin)) {
-          Firmata.write((byte)INPUT);
+          Firmata.write((byte)PIN_MODE_INPUT);
           Firmata.write(1);
           Firmata.write((byte)PIN_MODE_PULLUP);
           Firmata.write(1);
-          Firmata.write((byte)OUTPUT);
+          Firmata.write((byte)PIN_MODE_OUTPUT);
           Firmata.write(1);
         }
         if (IS_PIN_ANALOG(pin)) {
           Firmata.write(PIN_MODE_ANALOG);
+        #ifdef DEFAULT_ADC_RESOLUTION
+          Firmata.write(DEFAULT_ADC_RESOLUTION);
+        #else
           Firmata.write(10); // 10 = 10-bit resolution
+        #endif
         }
         if (IS_PIN_PWM(pin)) {
           Firmata.write(PIN_MODE_PWM);
           Firmata.write(DEFAULT_PWM_RESOLUTION);
         }
-        if (IS_PIN_DIGITAL(pin)) {
+        if (IS_PIN_SERVO(pin)) {
           Firmata.write(PIN_MODE_SERVO);
           Firmata.write(14);
         }
@@ -755,7 +767,7 @@ void systemResetCallback()
       setPinModeCallback(i, PIN_MODE_ANALOG);
     } else if (IS_PIN_DIGITAL(i)) {
       // sets the output to 0, configures portConfigInputs
-      setPinModeCallback(i, OUTPUT);
+      setPinModeCallback(i, PIN_MODE_OUTPUT);
     }
 
     servoPinMap[i] = 255;
diff --git a/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino b/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino
index faf2bec9..cc459500 100644
--- a/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino
+++ b/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino
@@ -140,7 +140,7 @@ int connectionAttempts = 0;
 bool streamConnected = false;
 
 /* analog inputs */
-int analogInputsToReport = 0;      // bitwise array to store pin reporting
+int analogInputsToReport = 0;       // bitwise array to store pin reporting
 
 /* digital input ports */
 byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
@@ -152,7 +152,7 @@ byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything
 /* 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)
+unsigned int samplingInterval = 19; // how often to sample analog inputs and i2c (in ms)
 
 /* i2c data */
 struct i2c_device_info {
@@ -373,7 +373,8 @@ void setPinModeCallback(byte pin, int mode)
     }
   }
   if (IS_PIN_ANALOG(pin)) {
-    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+    // turn on/off reporting
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0);
   }
   if (IS_PIN_DIGITAL(pin)) {
     if (mode == PIN_MODE_INPUT || mode == PIN_MODE_PULLUP) {

From d072c0ac13ab3dbd6a9643f6ddacdfda5759c5ad Mon Sep 17 00:00:00 2001
From: "Jens B." <jnsbyr@gmail.com>
Date: Wed, 3 Jan 2024 19:57:48 +0100
Subject: [PATCH 12/12] WiFiStream: make connection status const remapping from
 SDK to Firmata explicit for ESP8266

---
 utility/WiFiStream.h | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/utility/WiFiStream.h b/utility/WiFiStream.h
index a3b35715..2664e8ed 100644
--- a/utility/WiFiStream.h
+++ b/utility/WiFiStream.h
@@ -138,7 +138,14 @@ class WiFiStream : public Stream
   inline uint8_t status()
   {
   #ifdef ESP8266
-    return _client.status();
+    uint8 cs = _client.status();
+    switch (cs)
+    {
+      case CLOSED:      cs = STATUS_CLOSED; break;
+      case ESTABLISHED: cs = STATUS_ESTABLISHED; break;
+      default: break;
+    }
+    return cs;
   #elif ESP32
     return _client.connected()? STATUS_ESTABLISHED : STATUS_CLOSED;
   #endif