Merge pull request firmata#364 from zfields/bug

codec bug

Author: soundanalogous

Firmata.cpp

@@ -50,7 +50,7 @@ systemCallbackFunction FirmataClass::currentSystemResetCallback = (systemCallbac
  */
 void FirmataClass::sendValueAsTwo7bitBytes(int value)
 {
-  marshaller.transformByteStreamToMessageBytes(sizeof(value), reinterpret_cast<uint8_t *>(&value), sizeof(value));
+  marshaller.encodeByteStream(sizeof(value), reinterpret_cast<uint8_t *>(&value), sizeof(value));
 }
 
 /**

Firmata.h

@@ -128,7 +128,7 @@ class FirmataClass
 
     /* private methods ------------------------------ */
     void strobeBlinkPin(byte pin, int count, int onInterval, int offInterval);
-    friend void FirmataMarshaller::transformByteStreamToMessageBytes (size_t bytec, uint8_t * bytev, size_t max_bytes = 0) const;
+    friend void FirmataMarshaller::encodeByteStream (size_t bytec, uint8_t * bytev, size_t max_bytes = 0) const;
 
     /* callback functions */
     static callbackFunction currentAnalogCallback;

FirmataMarshaller.cpp

@@ -79,7 +79,7 @@ const
   FirmataStream->write(START_SYSEX);
   FirmataStream->write(EXTENDED_ANALOG);
   FirmataStream->write(pin);
-  transformByteStreamToMessageBytes(bytec, bytev, bytec);
+  encodeByteStream(bytec, bytev, bytec);
   FirmataStream->write(END_SYSEX);
 }
 
@@ -89,7 +89,7 @@ const
  * @param bytev A pointer to the array of data bytes to send in the message.
  * @param max_bytes Force message to be n bytes, regardless of data bits.
  */
-void FirmataMarshaller::transformByteStreamToMessageBytes (size_t bytec, uint8_t * bytev, size_t max_bytes)
+void FirmataMarshaller::encodeByteStream (size_t bytec, uint8_t * bytev, size_t max_bytes)
 const
 {
   static const size_t transmit_bits = 7;
@@ -248,7 +248,7 @@ const
   if ( (Stream *)NULL == FirmataStream ) { return; }
   if ( (0xF >= pin) && (0x3FFF >= value) ) {
     FirmataStream->write(ANALOG_MESSAGE|pin);
-    transformByteStreamToMessageBytes(sizeof(value), reinterpret_cast<uint8_t *>(&value), sizeof(value));
+    encodeByteStream(sizeof(value), reinterpret_cast<uint8_t *>(&value), sizeof(value));
   } else {
     sendExtendedAnalog(pin, sizeof(value), reinterpret_cast<uint8_t *>(&value));
   }
@@ -306,7 +306,7 @@ const
   FirmataStream->write(DIGITAL_MESSAGE | (portNumber & 0xF));
   // Tx bits  0-6 (protocol v1 and higher)
   // Tx bits 7-13 (bit 7 only for protocol v2 and higher)
-  transformByteStreamToMessageBytes(sizeof(portData), reinterpret_cast<uint8_t *>(&portData), sizeof(portData));
+  encodeByteStream(sizeof(portData), reinterpret_cast<uint8_t *>(&portData), sizeof(portData));
 }
 
 /**
@@ -326,7 +326,7 @@ const
   FirmataStream->write(major);
   FirmataStream->write(minor);
   for (i = 0; i < bytec; ++i) {
-    transformByteStreamToMessageBytes(sizeof(bytev[i]), reinterpret_cast<uint8_t *>(&bytev[i]), sizeof(bytev[i]));
+    encodeByteStream(sizeof(bytev[i]), reinterpret_cast<uint8_t *>(&bytev[i]));
   }
   FirmataStream->write(END_SYSEX);
 }
@@ -393,7 +393,7 @@ const
   FirmataStream->write(START_SYSEX);
   FirmataStream->write(command);
   for (i = 0; i < bytec; ++i) {
-    transformByteStreamToMessageBytes(sizeof(bytev[i]), reinterpret_cast<uint8_t *>(&bytev[i]), sizeof(bytev[i]));
+    encodeByteStream(sizeof(bytev[i]), reinterpret_cast<uint8_t *>(&bytev[i]));
   }
   FirmataStream->write(END_SYSEX);
 }

FirmataMarshaller.h

@@ -64,7 +64,7 @@ class FirmataMarshaller
     void reportAnalog(uint8_t pin, bool stream_enable) const;
     void reportDigitalPort(uint8_t portNumber, bool stream_enable) const;
     void sendExtendedAnalog(uint8_t pin, size_t bytec, uint8_t * bytev) const;
-    void transformByteStreamToMessageBytes (size_t bytec, uint8_t * bytev, size_t max_bytes = 0) const;
+    void encodeByteStream (size_t bytec, uint8_t * bytev, size_t max_bytes = 0) const;
 
     Stream * FirmataStream;
 };

FirmataParser.cpp

@@ -400,6 +400,25 @@ bool FirmataParser::bufferDataAtPosition(const uint8_t data, const size_t pos)
   return bufferOverflow;
 }
 
+/**
+ * Transform 7-bit firmata message into 8-bit stream
+ * @param bytec The encoded data byte length of the message (max: 16383).
+ * @param bytev A pointer to the encoded array of data bytes.
+ * @return The length of the decoded data.
+ * @note The conversion will be done in place on the provided buffer.
+ * @private
+ */
+size_t FirmataParser::decodeByteStream(size_t bytec, uint8_t * bytev) {
+  size_t decoded_bytes, i;
+
+  for ( i = 0, decoded_bytes = 0 ; i < bytec ; ++decoded_bytes, ++i ) {
+    bytev[decoded_bytes] = bytev[i];
+    bytev[decoded_bytes] |= (uint8_t)(bytev[++i] << 7);
+  }
+
+  return decoded_bytes;
+}
+
 /**
  * Process incoming sysex messages. Handles REPORT_FIRMWARE and STRING_DATA internally.
  * Calls callback function for STRING_DATA and all other sysex messages.
@@ -410,39 +429,25 @@ void FirmataParser::processSysexMessage(void)
   switch (dataBuffer[0]) { //first byte in buffer is command
     case REPORT_FIRMWARE:
       if (currentReportFirmwareCallback) {
-        size_t sv_major = dataBuffer[1], sv_minor = dataBuffer[2];
-        size_t i = 0, j = 3;
-        while (j < sysexBytesRead) {
-          // The string length will only be at most half the size of the
-          // stored input buffer so we can decode the string within the buffer.
-          bufferDataAtPosition(dataBuffer[j], i);
-          ++i;
-          ++j;
+        const size_t major_version_offset = 1;
+        const size_t minor_version_offset = 2;
+        const size_t string_offset = 3;
+        // Test for malformed REPORT_FIRMWARE message (used to query firmware prior to Firmata v3.0.0)
+        if ( 3 > sysexBytesRead ) {
+          (*currentReportFirmwareCallback)(currentReportFirmwareCallbackContext, 0, 0, (const char *)NULL);
+        } else {
+          const size_t end_of_string = (string_offset + decodeByteStream((sysexBytesRead - string_offset), &dataBuffer[string_offset]));
+          bufferDataAtPosition('\0', end_of_string); // NULL terminate the string
+          (*currentReportFirmwareCallback)(currentReportFirmwareCallbackContext, (size_t)dataBuffer[major_version_offset], (size_t)dataBuffer[minor_version_offset], (const char *)&dataBuffer[string_offset]);
         }
-        bufferDataAtPosition('\0', i); // Terminate the string
-        (*currentReportFirmwareCallback)(currentReportFirmwareCallbackContext, sv_major, sv_minor, (const char *)&dataBuffer[0]);
       }
       break;
     case STRING_DATA:
       if (currentStringCallback) {
-        size_t bufferLength = (sysexBytesRead - 1) / 2;
-        size_t i = 1, j = 0;
-        while (j < bufferLength) {
-          // The string length will only be at most half the size of the
-          // stored input buffer so we can decode the string within the buffer.
-          bufferDataAtPosition(dataBuffer[i], j);
-          ++i;
-          bufferDataAtPosition((dataBuffer[j] + (dataBuffer[i] << 7)), j);
-          ++i;
-          ++j;
-        }
-        // Make sure string is null terminated. This may be the case for data
-        // coming from client libraries in languages that don't null terminate
-        // strings.
-        if (dataBuffer[j - 1] != '\0') {
-          bufferDataAtPosition('\0', j);
-        }
-        (*currentStringCallback)(currentStringCallbackContext, (const char *)&dataBuffer[0]);
+        const size_t string_offset = 1;
+        const size_t end_of_string = (string_offset + decodeByteStream((sysexBytesRead - string_offset), &dataBuffer[string_offset]));
+        bufferDataAtPosition('\0', end_of_string); // NULL terminate the string
+        (*currentStringCallback)(currentStringCallbackContext, (const char *)&dataBuffer[string_offset]);
       }
       break;
     default:

FirmataParser.h

@@ -95,6 +95,7 @@ class FirmataParser
 
     /* private methods ------------------------------ */
     bool bufferDataAtPosition(const uint8_t data, const size_t pos);
+    size_t decodeByteStream(size_t bytec, uint8_t * bytev);
     void processSysexMessage(void);
     void systemReset(void);
 };