From 38151f002867930141b3375d445a855071a76457 Mon Sep 17 00:00:00 2001
From: Patrick Grawehr <pgrawehr@hotmail.com>
Date: Sun, 11 Jul 2021 14:40:59 +0200
Subject: [PATCH] Clear command requires only 2 bytes

---
 src/DigitalInputFirmata.cpp |  6 +++++-
 src/Frequency.cpp           | 21 +++++++++++++--------
 2 files changed, 18 insertions(+), 9 deletions(-)

diff --git a/src/DigitalInputFirmata.cpp b/src/DigitalInputFirmata.cpp
index 00845f1..5eec309 100644
--- a/src/DigitalInputFirmata.cpp
+++ b/src/DigitalInputFirmata.cpp
@@ -84,7 +84,10 @@ void DigitalInputFirmata::reportDigital(byte port, int value)
 {
   if (port < TOTAL_PORTS) {
     reportPINs[port] = (byte)value;
-    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
+    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
@@ -108,6 +111,7 @@ boolean DigitalInputFirmata::handlePinMode(byte pin, int mode)
       return true;
     } else {
       portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+      return true;
     }
   }
   return false;
diff --git a/src/Frequency.cpp b/src/Frequency.cpp
index cbcf86f..9b96913 100644
--- a/src/Frequency.cpp
+++ b/src/Frequency.cpp
@@ -44,25 +44,30 @@ boolean Frequency::handleSysex(byte command, byte argc, byte* argv)
   {
 	  return false;
   }
-  if (argc >= 5) // Expected: A command byte, a pin, the mode and a packed short
+  if (argc >= 2)
   {
 	  byte frequencyCommand = argv[0];
 	  byte pin = argv[1];
-	  int interrupt = digitalPinToInterrupt(pin);
-	  byte mode = argv[2]; // see below
-	  int32_t ms = (argv[4] << 7) | argv[3];
-	  // Clear
 	  if (frequencyCommand == FREQUENCY_SUBCOMMAND_CLEAR)
 	  {
-		  // This does not need a valid pin, so that it is possible to reset the interrupt even if the client doesn't know the state.
-		  if (_activePin >= 0)
+		  if (_activePin == pin || (_activePin >= 0 && pin == 0x7F))
 		  {
+		  	// This cannot be -1 here
+			  uint8_t interrupt = (uint8_t)digitalPinToInterrupt(_activePin);
 			  detachInterrupt(interrupt);
 			  _activePin = -1;
 		  }
 	  }
+  }
+  if (argc >= 5) // Expected: A command byte, a pin, the mode and a packed short
+  {
+	  byte frequencyCommand = argv[0];
+	  byte pin = argv[1];
+	  int interrupt = digitalPinToInterrupt(pin);
+	  byte mode = argv[2]; // see below
+	  int32_t ms = (argv[4] << 7) | argv[3];
 	  // Set or query
-	  else if (frequencyCommand == FREQUENCY_SUBCOMMAND_QUERY)
+	  if (frequencyCommand == FREQUENCY_SUBCOMMAND_QUERY)
 	  {
 		  if (pin >= TOTAL_PINS || interrupt < 0)
 		  {