diff --git a/DCCEXParser.cpp b/DCCEXParser.cpp index 2b90136..d16b21e 100644 --- a/DCCEXParser.cpp +++ b/DCCEXParser.cpp @@ -158,6 +158,66 @@ int DCCEXParser::splitValues(int result[MAX_PARAMS], const byte *cmd) return parameterCount; } +int DCCEXParser::splitHexValues(int result[MAX_PARAMS], const byte *cmd) +{ + byte state = 1; + byte parameterCount = 0; + int runningValue = 0; + const byte *remainingCmd = cmd + 1; // skips the opcode + + // clear all parameters in case not enough found + for (int i = 0; i < MAX_PARAMS; i++) + result[i] = 0; + + while (parameterCount < MAX_PARAMS) + { + byte hot = *remainingCmd; + + switch (state) + { + + case 1: // skipping spaces before a param + if (hot == ' ') + break; + if (hot == '\0' || hot == '>') + return parameterCount; + state = 2; + continue; + + case 2: // checking first hex digit + runningValue = 0; + state = 3; + continue; + + case 3: // building a parameter + if (hot >= '0' && hot <= '9') + { + runningValue = 16 * runningValue + (hot - '0'); + break; + } + if (hot >= 'A' && hot <= 'F') + { + runningValue = 16 * runningValue + 10 + (hot - 'A'); + break; + } + if (hot >= 'a' && hot <= 'f') + { + runningValue = 16 * runningValue + 10 + (hot - 'a'); + break; + } + if (hot==' ' || hot=='>' || hot=='\0') { + result[parameterCount] = runningValue; + parameterCount++; + state = 1; + continue; + } + return -1; // invalid hex digit + } + remainingCmd++; + } + return parameterCount; +} + FILTER_CALLBACK DCCEXParser::filterCallback = 0; AT_COMMAND_CALLBACK DCCEXParser::atCommandCallback = 0; void DCCEXParser::setFilter(FILTER_CALLBACK filter) @@ -265,6 +325,21 @@ void DCCEXParser::parse(Print *stream, byte *com, bool blocking) DCC::writeCVBitMain(p[0], p[1], p[2], p[3]); return; + case 'M': // WRITE TRANSPARENT DCC PACKET MAIN + case 'P': // WRITE TRANSPARENT DCC PACKET PROG

+ // Re-parse the command using a hex-only splitter + params=splitHexValues(p,com)-1; // drop REG + if (params<1) break; + { + byte packet[params]; + for (int i=0;i if (!stashCallback(stream, p)) break; diff --git a/DCCEXParser.h b/DCCEXParser.h index 2c2f337..ec0286d 100644 --- a/DCCEXParser.h +++ b/DCCEXParser.h @@ -40,6 +40,7 @@ struct DCCEXParser bool inCommandPayload=false; byte buffer[MAX_BUFFER+2]; int splitValues( int result[MAX_PARAMS], const byte * command); + int splitHexValues( int result[MAX_PARAMS], const byte * command); bool parseT(Print * stream, int params, int p[]); bool parseZ(Print * stream, int params, int p[]);