Arduino/libraries/Firmata/FirmataParser.cpp

   1 /*
   2   FirmataParser.cpp
   3   Copyright (c) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
   4   Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
   5
   6   This library is free software; you can redistribute it and/or
   7   modify it under the terms of the GNU Lesser General Public
   8   License as published by the Free Software Foundation; either
   9   version 2.1 of the License, or (at your option) any later version.
  10
  11   See file LICENSE.txt for further informations on licensing terms.
  12 */
  13
  14 //******************************************************************************
  15 //* Includes
  16 //******************************************************************************
  17
  18 #include "FirmataParser.h"
  19
  20 #include "FirmataConstants.h"
  21
  22 using namespace firmata;
  23
  24 //******************************************************************************
  25 //* Constructors
  26 //******************************************************************************
  27
  28 /**
  29  * The FirmataParser class.
  30  * @param dataBuffer A pointer to an external buffer used to store parsed data
  31  * @param dataBufferSize The size of the external buffer
  32  */
  33 FirmataParser::FirmataParser(uint8_t * const dataBuffer, size_t dataBufferSize)
  34 :
  35   dataBuffer(dataBuffer),
  36   dataBufferSize(dataBufferSize),
  37   executeMultiByteCommand(0),
  38   multiByteChannel(0),
  39   waitForData(0),
  40   parsingSysex(false),
  41   sysexBytesRead(0),
  42   currentAnalogCallbackContext((void *)NULL),
  43   currentDigitalCallbackContext((void *)NULL),
  44   currentReportAnalogCallbackContext((void *)NULL),
  45   currentReportDigitalCallbackContext((void *)NULL),
  46   currentPinModeCallbackContext((void *)NULL),
  47   currentPinValueCallbackContext((void *)NULL),
  48   currentReportFirmwareCallbackContext((void *)NULL),
  49   currentReportVersionCallbackContext((void *)NULL),
  50   currentDataBufferOverflowCallbackContext((void *)NULL),
  51   currentStringCallbackContext((void *)NULL),
  52   currentSysexCallbackContext((void *)NULL),
  53   currentSystemResetCallbackContext((void *)NULL),
  54   currentAnalogCallback((callbackFunction)NULL),
  55   currentDigitalCallback((callbackFunction)NULL),
  56   currentReportAnalogCallback((callbackFunction)NULL),
  57   currentReportDigitalCallback((callbackFunction)NULL),
  58   currentPinModeCallback((callbackFunction)NULL),
  59   currentPinValueCallback((callbackFunction)NULL),
  60   currentDataBufferOverflowCallback((dataBufferOverflowCallbackFunction)NULL),
  61   currentStringCallback((stringCallbackFunction)NULL),
  62   currentSysexCallback((sysexCallbackFunction)NULL),
  63   currentReportFirmwareCallback((versionCallbackFunction)NULL),
  64   currentReportVersionCallback((systemCallbackFunction)NULL),
  65   currentSystemResetCallback((systemCallbackFunction)NULL)
  66 {
  67     allowBufferUpdate = ((uint8_t *)NULL == dataBuffer);
  68 }
  69
  70 //******************************************************************************
  71 //* Public Methods
  72 //******************************************************************************
  73
  74 //------------------------------------------------------------------------------
  75 // Serial Receive Handling
  76
  77 /**
  78  * Parse data from the input stream.
  79  * @param inputData A single byte to be added to the parser.
  80  */
  81 void FirmataParser::parse(uint8_t inputData)
  82 {
  83   uint8_t command;
  84
  85   if (parsingSysex) {
  86     if (inputData == END_SYSEX) {
  87       //stop sysex byte
  88       parsingSysex = false;
  89       //fire off handler function
  90       processSysexMessage();
  91     } else {
  92       //normal data byte - add to buffer
  93       bufferDataAtPosition(inputData, sysexBytesRead);
  94       ++sysexBytesRead;
  95     }
  96   } else if ( (waitForData > 0) && (inputData < 128) ) {
  97     --waitForData;
  98     bufferDataAtPosition(inputData, waitForData);
  99     if ( (waitForData == 0) && executeMultiByteCommand ) { // got the whole message
 100       switch (executeMultiByteCommand) {
 101         case ANALOG_MESSAGE:
 102           if (currentAnalogCallback) {
 103             (*currentAnalogCallback)(currentAnalogCallbackContext,
 104                                      multiByteChannel,
 105                                      (dataBuffer[0] << 7)
 106                                      + dataBuffer[1]);
 107           }
 108           break;
 109         case DIGITAL_MESSAGE:
 110           if (currentDigitalCallback) {
 111             (*currentDigitalCallback)(currentDigitalCallbackContext,
 112                                       multiByteChannel,
 113                                       (dataBuffer[0] << 7)
 114                                       + dataBuffer[1]);
 115           }
 116           break;
 117         case SET_PIN_MODE:
 118           if (currentPinModeCallback)
 119             (*currentPinModeCallback)(currentPinModeCallbackContext, dataBuffer[1], dataBuffer[0]);
 120           break;
 121         case SET_DIGITAL_PIN_VALUE:
 122           if (currentPinValueCallback)
 123             (*currentPinValueCallback)(currentPinValueCallbackContext, dataBuffer[1], dataBuffer[0]);
 124           break;
 125         case REPORT_ANALOG:
 126           if (currentReportAnalogCallback)
 127             (*currentReportAnalogCallback)(currentReportAnalogCallbackContext, multiByteChannel, dataBuffer[0]);
 128           break;
 129         case REPORT_DIGITAL:
 130           if (currentReportDigitalCallback)
 131             (*currentReportDigitalCallback)(currentReportDigitalCallbackContext, multiByteChannel, dataBuffer[0]);
 132           break;
 133       }
 134       executeMultiByteCommand = 0;
 135     }
 136   } else {
 137     // remove channel info from command byte if less than 0xF0
 138     if (inputData < 0xF0) {
 139       command = inputData & 0xF0;
 140       multiByteChannel = inputData & 0x0F;
 141     } else {
 142       command = inputData;
 143       // commands in the 0xF* range don't use channel data
 144     }
 145     switch (command) {
 146       case ANALOG_MESSAGE:
 147       case DIGITAL_MESSAGE:
 148       case SET_PIN_MODE:
 149       case SET_DIGITAL_PIN_VALUE:
 150         waitForData = 2; // two data bytes needed
 151         executeMultiByteCommand = command;
 152         break;
 153       case REPORT_ANALOG:
 154       case REPORT_DIGITAL:
 155         waitForData = 1; // one data byte needed
 156         executeMultiByteCommand = command;
 157         break;
 158       case START_SYSEX:
 159         parsingSysex = true;
 160         sysexBytesRead = 0;
 161         break;
 162       case SYSTEM_RESET:
 163         systemReset();
 164         break;
 165       case REPORT_VERSION:
 166         if (currentReportVersionCallback)
 167           (*currentReportVersionCallback)(currentReportVersionCallbackContext);
 168         break;
 169     }
 170   }
 171 }
 172
 173 /**
 174  * @return Returns true if the parser is actively parsing data.
 175  */
 176 bool FirmataParser::isParsingMessage(void)
 177 const
 178 {
 179   return (waitForData > 0 || parsingSysex);
 180 }
 181
 182 /**
 183  * Provides a mechanism to either set or update the working buffer of the parser.
 184  * The method will be enabled when no buffer has been provided, or an overflow
 185  * condition exists.
 186  * @param dataBuffer A pointer to an external buffer used to store parsed data
 187  * @param dataBufferSize The size of the external buffer
 188  */
 189 int FirmataParser::setDataBufferOfSize(uint8_t * dataBuffer, size_t dataBufferSize)
 190 {
 191     int result;
 192
 193     if ( !allowBufferUpdate ) {
 194       result = __LINE__;
 195     } else if ((uint8_t *)NULL == dataBuffer) {
 196       result = __LINE__;
 197     } else {
 198       this->dataBuffer = dataBuffer;
 199       this->dataBufferSize = dataBufferSize;
 200       allowBufferUpdate = false;
 201       result = 0;
 202     }
 203
 204     return result;
 205 }
 206
 207 /**
 208  * Attach a generic sysex callback function to a command (options are: ANALOG_MESSAGE,
 209  * DIGITAL_MESSAGE, REPORT_ANALOG, REPORT DIGITAL, SET_PIN_MODE and SET_DIGITAL_PIN_VALUE).
 210  * @param command The ID of the command to attach a callback function to.
 211  * @param newFunction A reference to the callback function to attach.
 212  * @param context An optional context to be provided to the callback function (NULL by default).
 213  * @note The context parameter is provided so you can pass a parameter, by reference, to
 214  *       your callback function.
 215  */
 216 void FirmataParser::attach(uint8_t command, callbackFunction newFunction, void * context)
 217 {
 218   switch (command) {
 219     case ANALOG_MESSAGE:
 220       currentAnalogCallback = newFunction;
 221       currentAnalogCallbackContext = context;
 222       break;
 223     case DIGITAL_MESSAGE:
 224       currentDigitalCallback = newFunction;
 225       currentDigitalCallbackContext = context;
 226       break;
 227     case REPORT_ANALOG:
 228       currentReportAnalogCallback = newFunction;
 229       currentReportAnalogCallbackContext = context;
 230       break;
 231     case REPORT_DIGITAL:
 232       currentReportDigitalCallback = newFunction;
 233       currentReportDigitalCallbackContext = context;
 234       break;
 235     case SET_PIN_MODE:
 236       currentPinModeCallback = newFunction;
 237       currentPinModeCallbackContext = context;
 238       break;
 239     case SET_DIGITAL_PIN_VALUE:
 240       currentPinValueCallback = newFunction;
 241       currentPinValueCallbackContext = context;
 242       break;
 243   }
 244 }
 245
 246 /**
 247  * Attach a version callback function (supported option: REPORT_FIRMWARE).
 248  * @param command The ID of the command to attach a callback function to.
 249  * @param newFunction A reference to the callback function to attach.
 250  * @param context An optional context to be provided to the callback function (NULL by default).
 251  * @note The context parameter is provided so you can pass a parameter, by reference, to
 252  *       your callback function.
 253  */
 254 void FirmataParser::attach(uint8_t command, versionCallbackFunction newFunction, void * context)
 255 {
 256   switch (command) {
 257     case REPORT_FIRMWARE:
 258       currentReportFirmwareCallback = newFunction;
 259       currentReportFirmwareCallbackContext = context;
 260       break;
 261   }
 262 }
 263
 264 /**
 265  * Attach a system callback function (supported options are: SYSTEM_RESET, REPORT_VERSION).
 266  * @param command The ID of the command to attach a callback function to.
 267  * @param newFunction A reference to the callback function to attach.
 268  * @param context An optional context to be provided to the callback function (NULL by default).
 269  * @note The context parameter is provided so you can pass a parameter, by reference, to
 270  *       your callback function.
 271  */
 272 void FirmataParser::attach(uint8_t command, systemCallbackFunction newFunction, void * context)
 273 {
 274   switch (command) {
 275     case REPORT_VERSION:
 276       currentReportVersionCallback = newFunction;
 277       currentReportVersionCallbackContext = context;
 278       break;
 279     case SYSTEM_RESET:
 280       currentSystemResetCallback = newFunction;
 281       currentSystemResetCallbackContext = context;
 282       break;
 283   }
 284 }
 285
 286 /**
 287  * Attach a callback function for the STRING_DATA command.
 288  * @param command Must be set to STRING_DATA or it will be ignored.
 289  * @param newFunction A reference to the string callback function to attach.
 290  * @param context An optional context to be provided to the callback function (NULL by default).
 291  * @note The context parameter is provided so you can pass a parameter, by reference, to
 292  *       your callback function.
 293  */
 294 void FirmataParser::attach(uint8_t command, stringCallbackFunction newFunction, void * context)
 295 {
 296   switch (command) {
 297     case STRING_DATA:
 298       currentStringCallback = newFunction;
 299       currentStringCallbackContext = context;
 300       break;
 301   }
 302 }
 303
 304 /**
 305  * Attach a generic sysex callback function to sysex command.
 306  * @param command The ID of the command to attach a callback function to.
 307  * @param newFunction A reference to the sysex callback function to attach.
 308  * @param context An optional context to be provided to the callback function (NULL by default).
 309  * @note The context parameter is provided so you can pass a parameter, by reference, to
 310  *       your callback function.
 311  */
 312 void FirmataParser::attach(uint8_t command, sysexCallbackFunction newFunction, void * context)
 313 {
 314   (void)command;
 315   currentSysexCallback = newFunction;
 316   currentSysexCallbackContext = context;
 317 }
 318
 319 /**
 320  * Attach a buffer overflow callback
 321  * @param newFunction A reference to the buffer overflow callback function to attach.
 322  * @param context An optional context to be provided to the callback function (NULL by default).
 323  * @note The context parameter is provided so you can pass a parameter, by reference, to
 324  *       your callback function.
 325  */
 326 void FirmataParser::attach(dataBufferOverflowCallbackFunction newFunction, void * context)
 327 {
 328   currentDataBufferOverflowCallback = newFunction;
 329   currentDataBufferOverflowCallbackContext = context;
 330 }
 331
 332 /**
 333  * Detach a callback function for a specified command (such as SYSTEM_RESET, STRING_DATA,
 334  * ANALOG_MESSAGE, DIGITAL_MESSAGE, etc).
 335  * @param command The ID of the command to detatch the callback function from.
 336  */
 337 void FirmataParser::detach(uint8_t command)
 338 {
 339   switch (command) {
 340     case REPORT_FIRMWARE:
 341       attach(command, (versionCallbackFunction)NULL, NULL);
 342       break;
 343     case REPORT_VERSION:
 344     case SYSTEM_RESET:
 345       attach(command, (systemCallbackFunction)NULL, NULL);
 346       break;
 347     case STRING_DATA:
 348       attach(command, (stringCallbackFunction)NULL, NULL);
 349       break;
 350     case START_SYSEX:
 351       attach(command, (sysexCallbackFunction)NULL, NULL);
 352       break;
 353     default:
 354       attach(command, (callbackFunction)NULL, NULL);
 355       break;
 356   }
 357 }
 358
 359 /**
 360  * Detach the buffer overflow callback
 361  * @param <unused> Any pointer of type dataBufferOverflowCallbackFunction.
 362  */
 363 void FirmataParser::detach(dataBufferOverflowCallbackFunction)
 364 {
 365   currentDataBufferOverflowCallback = (dataBufferOverflowCallbackFunction)NULL;
 366   currentDataBufferOverflowCallbackContext = (void *)NULL;
 367 }
 368
 369 //******************************************************************************
 370 //* Private Methods
 371 //******************************************************************************
 372
 373 /**
 374  * Buffer abstraction to prevent memory corruption
 375  * @param data The byte to put into the buffer
 376  * @param pos The position to insert the byte into the buffer
 377  * @return writeError A boolean to indicate if an error occured
 378  * @private
 379  */
 380 bool FirmataParser::bufferDataAtPosition(const uint8_t data, const size_t pos)
 381 {
 382   bool bufferOverflow = (pos >= dataBufferSize);
 383
 384   // Notify of overflow condition
 385   if ( bufferOverflow
 386   && ((dataBufferOverflowCallbackFunction)NULL != currentDataBufferOverflowCallback) )
 387   {
 388     allowBufferUpdate = true;
 389     currentDataBufferOverflowCallback(currentDataBufferOverflowCallbackContext);
 390     // Check if overflow was resolved during callback
 391     bufferOverflow = (pos >= dataBufferSize);
 392   }
 393
 394   // Write data to buffer if no overflow condition persist
 395   if ( !bufferOverflow )
 396   {
 397     dataBuffer[pos] = data;
 398   }
 399
 400   return bufferOverflow;
 401 }
 402
 403 /**
 404  * Transform 7-bit firmata message into 8-bit stream
 405  * @param bytec The encoded data byte length of the message (max: 16383).
 406  * @param bytev A pointer to the encoded array of data bytes.
 407  * @return The length of the decoded data.
 408  * @note The conversion will be done in place on the provided buffer.
 409  * @private
 410  */
 411 size_t FirmataParser::decodeByteStream(size_t bytec, uint8_t * bytev) {
 412   size_t decoded_bytes, i;
 413
 414   for ( i = 0, decoded_bytes = 0 ; i < bytec ; ++decoded_bytes, ++i ) {
 415     bytev[decoded_bytes] = bytev[i];
 416     bytev[decoded_bytes] |= (uint8_t)(bytev[++i] << 7);
 417   }
 418
 419   return decoded_bytes;
 420 }
 421
 422 /**
 423  * Process incoming sysex messages. Handles REPORT_FIRMWARE and STRING_DATA internally.
 424  * Calls callback function for STRING_DATA and all other sysex messages.
 425  * @private
 426  */
 427 void FirmataParser::processSysexMessage(void)
 428 {
 429   switch (dataBuffer[0]) { //first byte in buffer is command
 430     case REPORT_FIRMWARE:
 431       if (currentReportFirmwareCallback) {
 432         const size_t major_version_offset = 1;
 433         const size_t minor_version_offset = 2;
 434         const size_t string_offset = 3;
 435         // Test for malformed REPORT_FIRMWARE message (used to query firmware prior to Firmata v3.0.0)
 436         if ( 3 > sysexBytesRead ) {
 437           (*currentReportFirmwareCallback)(currentReportFirmwareCallbackContext, 0, 0, (const char *)NULL);
 438         } else {
 439           const size_t end_of_string = (string_offset + decodeByteStream((sysexBytesRead - string_offset), &dataBuffer[string_offset]));
 440           bufferDataAtPosition('\0', end_of_string); // NULL terminate the string
 441           (*currentReportFirmwareCallback)(currentReportFirmwareCallbackContext, (size_t)dataBuffer[major_version_offset], (size_t)dataBuffer[minor_version_offset], (const char *)&dataBuffer[string_offset]);
 442         }
 443       }
 444       break;
 445     case STRING_DATA:
 446       if (currentStringCallback) {
 447         const size_t string_offset = 1;
 448         const size_t end_of_string = (string_offset + decodeByteStream((sysexBytesRead - string_offset), &dataBuffer[string_offset]));
 449         bufferDataAtPosition('\0', end_of_string); // NULL terminate the string
 450         (*currentStringCallback)(currentStringCallbackContext, (const char *)&dataBuffer[string_offset]);
 451       }
 452       break;
 453     default:
 454       if (currentSysexCallback)
 455         (*currentSysexCallback)(currentSysexCallbackContext, dataBuffer[0], sysexBytesRead - 1, dataBuffer + 1);
 456   }
 457 }
 458
 459 /**
 460  * Resets the system state upon a SYSTEM_RESET message from the host software.
 461  * @private
 462  */
 463 void FirmataParser::systemReset(void)
 464 {
 465   size_t i;
 466
 467   waitForData = 0; // this flag says the next serial input will be data
 468   executeMultiByteCommand = 0; // execute this after getting multi-byte data
 469   multiByteChannel = 0; // channel data for multiByteCommands
 470
 471   for (i = 0; i < dataBufferSize; ++i) {
 472     dataBuffer[i] = 0;
 473   }
 474
 475   parsingSysex = false;
 476   sysexBytesRead = 0;
 477
 478   if (currentSystemResetCallback)
 479     (*currentSystemResetCallback)(currentSystemResetCallbackContext);
 480 }