static void DumpNumber(lua_Number x, DumpState* D)
{
const int srcSize = sizeof(x);
const int dstSize = D->config->sizeof_lua_Number;
const int endianSwap = EndianSwapRequired(D->config->endianness);
lua_assert((dstSize == 4) || (dstSize == 8));
lua_assert(dstSize >= srcSize);
if (dstSize > srcSize)
{
const double x64 = x;
double out = x64;
if (endianSwap)
EndianSwapScalar(&out, &x64, sizeof(out));
DumpMem(&out, 1, sizeof(out), D);
}
else
{
lua_Number out = x;
if (endianSwap)
EndianSwapScalar(&out, &x, sizeof(out));
DumpMem(&out, 1, sizeof(out), D);
}
// Original Lua implmenetation:
// DumpVar(x, D);
}
static void DumpInt(int x, DumpState* D)
{
const int srcSize = sizeof(x);
const int dstSize = D->config->sizeof_int;
const int endianSwap = EndianSwapRequired(D->config->endianness);
lua_assert((dstSize == 4) || (dstSize == 8));
lua_assert(dstSize >= srcSize);
if (dstSize > srcSize)
{
const int64_t x64 = x;
int64_t out = x64;
if (endianSwap)
EndianSwapScalar(&out, &x64, sizeof(out));
DumpMem(&out, 1, sizeof(out), D);
}
else
{
int out = x;
if (endianSwap)
EndianSwapScalar(&out, &x, sizeof(out));
DumpMem(&out, 1, sizeof(out), D);
}
// Original Lua implementation:
// DumpVar(x, D);
}
static void DumpSizeT(size_t x, DumpState* D)
{
const int srcSize = sizeof(size_t);
const int dstSize = D->config->sizeof_size_t;
const int endianSwap = EndianSwapRequired(D->config->endianness);
lua_assert((dstSize == 4) || (dstSize == 8));
lua_assert(dstSize >= srcSize);
if (dstSize > srcSize)
{
const uint64_t x64 = x;
uint64_t out = x64;
if (endianSwap)
EndianSwapScalar(&out, &x64, sizeof(out));
DumpMem(&out, 1, sizeof(out), D);
}
else
{
size_t out = x;
if (endianSwap)
EndianSwapScalar(&out, &x, sizeof(out));
DumpMem(&out, 1, sizeof(out), D);
}
}
static void DumpNumber(lua_Number x, DumpState* D)
{
/*
http://stackoverflow.com/questions/14954088/how-do-i-handle-byte-order-differences-when-reading-writing-floating-point-types/14955046#14955046
10-byte little-endian serialized format for double:
- normalized mantissa stored as 64-bit (8-byte) signed integer:
negative range: (-2^53, -2^52]
zero: 0
positive range: [+2^52, +2^53)
- 16-bit (2-byte) signed exponent:
range: [-0x7FFE, +0x7FFE]
Represented value = mantissa * 2^(exponent - 53)
Special cases:
- +infinity: mantissa = 0x7FFFFFFFFFFFFFFF, exp = 0x7FFF
- -infinity: mantissa = 0x8000000000000000, exp = 0x7FFF
- NaN: mantissa = 0x0000000000000000, exp = 0x7FFF
- +/-0: only one zero supported
*/
double y=(double)x,m;
unsigned char z[8];
long long im;
int ie;
if (luai_numisnan(NULL,x)) { DumpChar(0,D); /* NaN */ return; }
else if (x == LUA_INFINITY) { DumpChar(1,D); /* +inf */ return; }
else if (x == -LUA_INFINITY) { DumpChar(2,D); /* -inf */ return; }
else if (x == 0) { DumpChar(3,D); /* 0 */ return; }
/* Split double into normalized mantissa (range: (-1, -0.5], 0, [+0.5, +1)) and base-2 exponent */
m = frexp(y, &ie); /* y = m * 2^ie exactly for FLT_RADIX=2, frexp() can't fail */
/* Extract most significant 53 bits of mantissa as integer */
m = ldexp(m, 53); /* can't overflow because DBL_MAX_10_EXP >= 37 equivalent to DBL_MAX_2_EXP >= 122 */
im = (long long)trunc(m); /* exact unless DBL_MANT_DIG > 53 */
/* If the exponent is too small or too big, reduce the number to 0 or +/- infinity */
if (ie>0x7FFE)
{
if (im<0) DumpChar(2,D); /* -inf */ else DumpChar(1,D); /* +inf */
return;
}
else if (ie<-0x7FFE) { DumpChar(3,D); /* 0 */ return; }
DumpChar(4,D); /* encoded */
/* Store im as signed 64-bit little-endian integer */
z[0]=(unsigned char)(im&0xFF);
z[1]=(unsigned char)((im>>8)&0xFF);
z[2]=(unsigned char)((im>>16)&0xFF);
z[3]=(unsigned char)((im>>24)&0xFF);
z[4]=(unsigned char)((im>>32)&0xFF);
z[5]=(unsigned char)((im>>40)&0xFF);
z[6]=(unsigned char)((im>>48)&0xFF);
z[7]=(unsigned char)((im>>56)&0xFF);
DumpMem(z,1,8,D);
/* Store ie as signed 16-bit little-endian integer */
z[0]=(unsigned char)(ie&0xFF);
z[1]=(unsigned char)((ie>>8)&0xFF);
DumpMem(z,1,2,D);
}
int SendBytesAndWaitForResponse(SOCKET destination,char *source, int nBytes, char *destinationBuffer, int MaxReadSize,int timeout)
{
int i=-1;
#ifdef WIN32
u_long tmp=1;
#else
int tmp=1;
#endif
fd_set fds;
struct timeval tv;
if (debug) {
printf("Sending..\n");
DumpMem(source,nBytes);
}
// if (send(destination, source, nBytes,0) >0) {
if (timeout>0)
{
#ifdef WIN32
ioctlsocket(destination, FIONBIO, &tmp);
#else
ioctl(destination, FIONBIO, (char *)&tmp);
#endif
send(destination, source, nBytes,0);
tv.tv_sec = timeout;
tv.tv_usec = 0;
FD_ZERO(&fds);
FD_SET(destination, &fds);
//printf("Esperando Timeout: %i\n",timeout);
i=select((int)destination+1,&fds,0,0,&tv);
//printf("saliendo select: %i\n",i);
if (i<=0) return(-1);
} else {
send(destination, source, nBytes,0);
}
i=recv(destination, (char *)destinationBuffer, MaxReadSize, 0);
if (timeout>0)
{
tmp=0;
#ifdef WIN32
ioctlsocket(destination, FIONBIO, &tmp);
#else
ioctl(destination, FIONBIO, (char *)&tmp);
#endif
}
if (debug)
{
printf("[*] received: %i bytes\n",i);
DumpMem(destinationBuffer,i);
}
// }
//printf("salimos...: %i\n",i);
return(i);
}
void CHttpStateMachine::process_state(t_sm_state state, bool entry)
{
(this->*m_process_state[state])(entry);
#ifdef __DEBUG__
int n = 0;
switch (state) {
case HTTP_STATE_START:
case HTTP_STATE_END:
case HTTP_STATE_RECEIVE_COMMAND_REPLY:
default:
n = 0;
break;
case HTTP_STATE_SEND_COMMAND_REQUEST:
n = m_packetLenght;
break;
case HTTP_STATE_PROCESS_COMMAND_REPLY:
n = m_lastRead;
break;
}
if (entry) {
DumpMem(m_buffer, n, m_state_name[state], m_ok);
} else {
AddDebugLogLineN(logProxy,
wxString(wxT("wait state -- ")) << m_state_name[state]);
}
#endif // __DEBUG__
}
static void DumpUInt(lu_int32 x, DumpState* D)
{
unsigned char y[4];
y[0]=(unsigned char)(x&0xFF);
y[1]=(unsigned char)((x>>8)&0xFF);
y[2]=(unsigned char)((x>>16)&0xFF);
y[3]=(unsigned char)((x>>24)&0xFF);
DumpMem(y,1,4,D);
}
void SendData( const void *buf, size_t bytesToWrite )
{
if ( gDebug )
{
DumpMem( "Send", 0, buf, bytesToWrite );
}
gSerialPort.Write( buf, bytesToWrite );
} // SendData
static void DumpInt(int x, DumpState* D)
{
unsigned char y[4];
if(x<0)
error(D,"negative int value");
y[0]=(unsigned char)(x&0xFF);
y[1]=(unsigned char)((x>>8)&0xFF);
y[2]=(unsigned char)((x>>16)&0xFF);
y[3]=(unsigned char)((x>>24)&0xFF);
DumpMem(y,1,4,D);
}
void BioloidBus::WriteBuffer()
{
if ( m_showPackets )
{
DumpMem( "W", 0, m_data, m_dataBytes );
}
WriteBufferedData( m_data, m_dataBytes );
m_dataBytes = 0;
}
smheader *BuildSmbPacket1(void)
{
char buf2[4096];
smheader *SmbPacket1;
memset((char*)buf2,'\0',sizeof(buf2));
BuildAuthRequest((tSmbNtlmAuthRequest*)buf2,0,NULL,NULL);
#ifdef _DBG_
DumpMem((char*)buf2,SmbLength((tSmbNtlmAuthRequest*)buf2));
#endif
SmbPacket1=BuildSmbPacket((smheader*)NULL,SESSIONSETUPANDX,0,buf2,40);
return(SmbPacket1);
}
static void DumpVector(const void* b, int n, size_t size, DumpState* D)
{
const int endianSwap = EndianSwapRequired(D->config->endianness);
DumpInt(n, D);
if (endianSwap)
{
void* out = malloc(n * size);
EndianSwapArray(out, b, n, size);
DumpMem(out, n, size, D);
free(out);
}
else
DumpMem(b, n, size, D);
// Original Lua implementation
// DumpInt(n, D);
// DumpMem(b, n, size, D);
}
void CSocks5StateMachine::process_state(t_sm_state state, bool entry)
{
/* Ok, the syntax is terrible, but this is correct. This is a
* pointer to a member function. No C equivalent for that. */
(this->*m_process_state[state])(entry);
#ifdef __DEBUG__
int n = 0;
switch (state) {
case SOCKS5_STATE_START:
case SOCKS5_STATE_END:
case SOCKS5_STATE_RECEIVE_AUTHENTICATION_METHOD:
case SOCKS5_STATE_RECEIVE_AUTHENTICATION_GSSAPI:
case SOCKS5_STATE_RECEIVE_AUTHENTICATION_USERNAME_PASSWORD:
case SOCKS5_STATE_RECEIVE_COMMAND_REPLY:
default:
n = 0;
break;
case SOCKS5_STATE_SEND_QUERY_AUTHENTICATION_METHOD:
case SOCKS5_STATE_SEND_AUTHENTICATION_GSSAPI:
case SOCKS5_STATE_SEND_AUTHENTICATION_USERNAME_PASSWORD:
case SOCKS5_STATE_SEND_COMMAND_REQUEST:
n = m_packetLenght;
break;
case SOCKS5_STATE_PROCESS_AUTHENTICATION_METHOD:
case SOCKS5_STATE_PROCESS_AUTHENTICATION_GSSAPI:
case SOCKS5_STATE_PROCESS_AUTHENTICATION_USERNAME_PASSWORD:
case SOCKS5_STATE_PROCESS_COMMAND_REPLY:
n = m_lastRead;
break;
}
if (entry) {
DumpMem(m_buffer, n, m_state_name[state], m_ok);
} else {
AddDebugLogLineN(logProxy,
wxString(wxT("wait state -- ")) << m_state_name[state]);
}
#endif // __DEBUG__
}
bool BioloidBus::ReadStatusPacket( BioloidPacket *pkt )
{
Bioloid::Error err;
m_dataBytes = 0;
do
{
uint8_t ch;
if ( !ReadByte( &ch ))
{
//LogError( "BioloidBus::ReadStatusPacket: ReadByte returned false\n");
return false;
}
if ( m_dataBytes < sizeof( m_data ))
{
m_data[ m_dataBytes++ ] = ch;
}
err = pkt->ProcessChar( ch );
} while ( err == Bioloid::ERROR_NOT_DONE );
if ( err == Bioloid::ERROR_NONE )
{
if ( m_showPackets )
{
if ( m_dataBytes > 0 )
{
DumpMem( "R", 0, m_data, m_dataBytes );
}
}
}
else
{
Log( "BioloidBus::ReadStatusPacket err = %d\n", err );
}
return err == Bioloid::ERROR_NONE;
}
static void DumpVector(const void* b, int n, size_t size, DumpState* D)
{
DumpInt(n,D);
DumpMem(b,n,size,D);
}
void I2C_SerialDongle::PacketReceived( PKT_Packet *packet )
{
uint8_t *pd = &packet->data[ 0 ];
PacketType packetType = (PacketType)*pd++;
uint8_t crc = 0;
uint8_t len = 0;
if ( m_debugDongleData )
{
DumpMem( "PktRcvd", 0, packet->data, packet->len );
}
if ( packetType == ErrorPacket )
{
m_response.m_errorCode = *pd;
LogError( "Rcvd Error packet, error code:%d '%s'\n", m_response.m_errorCode, ErrorStr( m_response.m_errorCode ));
}
else
if ( packetType == m_response.m_responseType )
{
// This is the type of packet that the caller is waiting for.
crc = m_response.m_partialCrc;
if ( packet->len > 1 )
{
// Check the CRC to see if it's valid
len = *pd; crc = Crc8( crc, *pd ); pd++;
// Sanity check the length. The packet length should equal the i2c len
// plus 3 (1 for packet type, 1 for len, 1 for CRC)
if (( len + 3 ) != packet->len )
{
LogError( "I2C_SerialDongle::PacketReceived Inconsistent packet length. Serial packetLen = %d, i2c packetLen = %d (expecting %d)\n",
packet->len, len, packet->len - 3 );
m_response.m_errorCode = I2C_ERROR_BAD_LEN;
packetType = ErrorPacket;
}
else
{
for ( int i = 0; i < len; i++ )
{
crc = Crc8( crc, pd[ i ]);
}
if ( crc != pd[ len ] )
{
LogError( "I2C_SerialDongle::PacketReceived CRC failure. Found 0x%02x, expecting 0x%02x\n", pd[ len ], crc );
m_response.m_errorCode = I2C_ERROR_BAD_CRC;
packetType = ErrorPacket;
}
}
// We've received a good packet, and we're waiting for this type
// of packet. Wakeup the reader
if ( len < 1 )
{
LogError( "I2C_SerialDongle::PacketReceived packet len too small (%d). Needs to be at least 1 to store address of sender\n", len );
len = 1;
}
if ( len > m_response.m_buf->dataLen )
{
LogError( "I2C_SerialDongle::PacketReceived packet buffer too small. bufferSize = %d, received %d\n", m_response.m_buf->dataLen, len );
len = m_response.m_buf->dataLen;
}
m_response.m_buf->dataLen = len;
if (( len > 0 ) && ( m_response.m_buf != NULL ))
{
memcpy( &m_response.m_buf->data[ 0 ], &pd[ 0 ], len );
}
}
}
if (( packetType == m_response.m_responseType ) || ( packetType == ErrorPacket ))
{
// Setting m_responseType to NoPacket signals that we're no longer
// waiting for a packet.
m_response.m_responseType = NoPacket;
sem_post( &m_response.m_semaphore );
return;
}
// Call the registered callback to report the received packet
if ( m_callback != NULL )
{
m_callback->PacketReceived( m_response.m_buf );
}
else
{
LogError( "m_callback == NULL\n" );
LogError( "packetType:%d m_response.m_responseType:%d\n", packetType, m_response.m_responseType );
LogError( "packet len: %d\n", packet->len );
DumpMem( " pkt", 0, packet->data, packet->len );
}
} // PacketReceived
/**
* Sends a packet and waits for a respomse. We need to do things in the
* correct order to prevent a race condition.
*/
bool I2C_SerialDongle::SendPacketWaitForResponse
(
const PKT_Packet *pkt,
PacketType responseType,
uint8_t partialCrc,
Buffer *buf
)
{
// Currently we only handle one outstanding "wait" at a time.
if ( m_response.m_responseType != NoPacket )
{
LogError( "m_response.m_responseType:%d\n", m_response.m_responseType );
}
assert( m_response.m_responseType == NoPacket );
// Setup the response before sending the packet. Since we have a different
// thread processing the received packets, we could receive the packet
// really quickly
m_response.m_responseType = responseType;
m_response.m_partialCrc = partialCrc;
m_response.m_buf = buf;
m_response.m_errorCode = I2C_ERROR_NONE;
// Send the packet
if ( m_debugDongleData )
{
DumpMem( "SendPkt", 0, pkt->data, pkt->len );
}
PKT_SendPacket( pkt, this );
// Wait for the response
struct timespec waitTime;
waitTime.tv_sec = time( NULL ) + 3; // We shouldn't have to wait this long for a response
waitTime.tv_nsec = 0;
if ( sem_timedwait( &m_response.m_semaphore, &waitTime ) != 0 )
{
if ( errno == ETIMEDOUT )
{
LogError( "I2C_Dongle::WaitForResponse timeout waiting for response\n" );
}
else
{
LogError( "I2C_Dongle::WaitForResponse: error waiting for semaphore: %s (%d)\n", strerror( errno ), errno );
}
m_response.m_responseType = NoPacket;
return false;
}
if ( m_response.m_errorCode != I2C_ERROR_NONE )
{
LogError( "I2C_Dongle::WaitForResponse error %d detected\n", m_response.m_errorCode );
return false;
}
return true;
} // WaitForResponse
static void DumpChar(int y, DumpState* D)
{
char x=(char)y;
DumpMem(&x, 1, sizeof(x), D);
}
int WriteRemoteFile(RELAY relay, smheader *buffer, char *lpFileName)
{
char path[256];
char buf[64000];
smheader *packet;
int i;
int filesize;
char *filedata;
uint16 FID;
uint16 TreeID;
sprintf(path,"\\\\%s\\admin$",inet_ntoa(relay.destinationaddr.sin_addr));
memset(buf,0,sizeof(buf));
i=BuildTreeConnectAndXStub(buf,"",path,"?????");
packet=BuildSmbPacket((smheader*)buffer,TREECONNETANDX,0,buf,i);//,(int)strlen(path));
printf("[+] Trying to connect to admin$\r");
if (debug)
{
CleanLine(verbose);
DumpMem((char*)packet,SmbPacketLen(packet));
}
i=SendBytesAndWaitForResponse(relay.destination,(char*)packet, SmbPacketLen(packet),(char*)buf,sizeof(buf),SMBWAITTIMEOUT);
if ((i<=0) || (((smheader*)buf)->NtStatus!=0x00000000) ){
CleanLine(verbose);
printf("[-] Error. Unable to connect to admin$\n");
return(0);
}
TreeID=((smheader*)buf)->TreeId;
sprintf(path,"\\%s",lpFileName);
filedata =ReadFileToSend(&filesize,lpFileName);
if(!filedata)
{
CleanLine(verbose);
printf("[-] Error. Unable to open %s\n",lpFileName);
return(0);
}
packet=BuildSmbPacket((smheader*)buf,NTCREATEANDX,0,path,filesize);
CleanLine(verbose);
printf("[+] Creating Remote File %s under admin$\r",lpFileName);
if (debug)
{
CleanLine(verbose);
DumpMem((char*)packet,SmbPacketLen(packet));
}
i=SendBytesAndWaitForResponse(relay.destination,(char*)packet, SmbPacketLen(packet),(char*)buf,sizeof(buf),SMBWAITTIMEOUT);
if ((i<=0) || (((smheader*)buf)->NtStatus!=0x00000000) ){
CleanLine(verbose);
if ( ((smheader*)buf)->NtStatus == STATUS_SHARING_VIOLATION) {
printf("[-] Remote File already in use (try to connect to the remote Shell).\n");
return(1);
}
printf("[-] Error. Unable to create file under admin$ (Error 0x%x)\n",((smheader*)buf)->NtStatus);
return(0);
}
memcpy((char*)&FID,((smheader*)buf)->buffer+6,2);
CleanLine(verbose);
printf("[+] Writing File %s into admin$ (%i bytes)\r",lpFileName,filesize);
packet=BuildSmbPacket((smheader*)buf,WRITEANDX,0,filedata,filesize);
free(filedata);
if (debug)
{
CleanLine(verbose);
DumpMem((char*)packet,SmbPacketLen(packet));
}
i=SendBytesAndWaitForResponse(relay.destination,(char*)packet, SmbPacketLen(packet),(char*)buf,sizeof(buf),SMBWAITTIMEOUT);
if ((i<=0) || (((smheader*)buf)->NtStatus!=0x00000000) ){
CleanLine(verbose);
printf("[-] Error. Unable to Write File.\n");
return(0);
}
packet=BuildSmbPacket((smheader*)buf,SMBCLOSE,0,&FID,2);
packet->TreeId=TreeID;
if (verbose)
{
CleanLine(verbose);
printf("[*] Closing File handle - FID: %2.2x\r",FID);
if (debug) DumpMem((char*)packet,SmbPacketLen(packet));
}
i=SendBytesAndWaitForResponse(relay.destination,(char*)packet, SmbPacketLen(packet),(char*)buf,sizeof(buf),SMBWAITTIMEOUT);
if ((i<=0) || (((smheader*)buf)->NtStatus!=0x00000000) ){
CleanLine(verbose);
printf("[-] Error Closing File Handle\n");
return(0);
}
return(1);
}
bool BioloidCommandLine::ProcessLine( char *lineStr )
{
char *devTypeStr;
BLD_DevType_t *devType;
char token[ 20 ];
StrTokenizer line( lineStr, token, sizeof( token ));
unsigned devTypeIdx;
if ( m_bus == NULL )
{
LogError( "SetBus not called\n" );
return false;
}
// Pull out the device type
if (( devTypeStr = line.NextToken( gDelim )) == NULL )
{
// Empty line - ignore
return true;
}
// Check for special non-device type commands
if ( strcmp( devTypeStr, "action" ) == 0 )
{
m_bus->SendAction();
// The action command is sent as a broadcast, so no response
// is expected.
return true;
}
if ( strcmp( devTypeStr, "scan" ) == 0 )
{
uint8_t numIds;
if ( !line.NextNum( &numIds ))
{
numIds = 32;
}
if ( numIds < 100 )
{
bool servoIdFound = m_bus->Scan( DevFound, 0, numIds );
bool sensorIdFound = m_bus->Scan( DevFound, 100, numIds );
if ( !servoIdFound && !sensorIdFound )
{
Log( "No devices found\n" );
}
}
else
{
if ( !m_bus->Scan( DevFound, 0 , numIds ))
{
Log( "No devices found\n" );
}
}
return true;
}
if ( strcmp( devTypeStr, "dev-types" ) == 0 )
{
for ( devTypeIdx = 0; devTypeIdx < m_numDevTypes; devTypeIdx++ )
{
devType = m_devType[devTypeIdx];
Log( "%-10s Model: %5u %2d registers\n",
devType->devTypeStr, devType->model, devType->numRegs );
}
return true;
}
if ( strcmp( devTypeStr, "quit" ) == 0 )
{
return false;
}
// Since it's not one of those - assume it's a device type
devType = NULL;
for ( devTypeIdx = 0; devTypeIdx < m_numDevTypes; devTypeIdx++ )
{
if ( strcmp( m_devType[ devTypeIdx ]->devTypeStr, devTypeStr ) == 0 )
{
devType = m_devType[ devTypeIdx ];
break;
}
}
if ( devTypeIdx >= m_numDevTypes )
{
LogError( "Unrecognized device type: '%s'\n", devTypeStr );
return true;
}
Bioloid::ID_t id;
if ( !line.NextNum( &id ))
{
if ( strcmp( line.PrevToken(), "reg" ) == 0 )
{
DumpRegInfo( devType );
return true;
}
LogError( "Invalid ID: '%s'\n", line.PrevToken() );
return true;
}
if ( id >= Bioloid::INVALID_ID )
{
LogError( "IDs must be 254 (0xFE) or less\n" );
return true;
}
m_device.SetBusAndID( m_bus, id );
char *cmdStr;
if (( cmdStr = line.NextToken()) == NULL )
{
LogError( "No command specified for %s %u\n", devType->devTypeStr, id );
return true;
}
LogVerbose( "DevType: %s ID: %d Cmd: %s\n", devType->devTypeStr, id, cmdStr );
if ( strcmp( cmdStr, "ping" ) == 0 )
{
Bioloid::Error err;
if ( id == Bioloid::BROADCAST_ID )
{
LogError( "Broadcast ID not valid with ping command\n" );
return true;
}
if (( err = m_device.Ping()) == Bioloid::ERROR_NONE )
{
Log( "%s %d Response Received\n", devType->devTypeStr, id );
}
else
{
Log( "%s %d ", devType->devTypeStr, id );
PrintError( err );
}
}
else
if (( strcmp( cmdStr, "read-data" ) == 0 )
|| ( strcmp( cmdStr, "rd" ) == 0 ))
{
uint8_t offset;
uint8_t numBytes;
if ( id == Bioloid::BROADCAST_ID )
{
LogError( "Broadcast ID not valid with read-data command\n" );
return true;
}
if ( !line.NextNum( &offset ))
{
LogError( "Invalid offset specified: '%s'\n", line.PrevToken() );
return true;
}
if ( !line.NextNum( &numBytes ))
{
LogError( "Invalid numBytes specified: '%s'\n", line.PrevToken() );
return true;
}
if ( numBytes > sizeof( gReadBuf ))
{
LogError( "Only able to a maximum of %d bytes\n", sizeof( gReadBuf ));
return true;
}
if ( PrintError( m_device.Read( offset, gReadBuf, numBytes )))
{
return true;
}
DumpMem( "Read", offset, gReadBuf, numBytes );
}
else
if (( strcmp( cmdStr, "write-data" ) == 0 )
|| ( strcmp( cmdStr, "wd" ) == 0 ))
{
uint8_t offset;
uint8_t numBytes;
uint8_t data[ 20 ];
if ( !ParseOffsetAndData( line, &offset, &numBytes, data, sizeof( data )))
{
return true;
}
PrintError( m_device.Write( offset, data, numBytes ));
}
else
if (( strcmp( cmdStr, "reg-write" ) == 0 )
|| ( strcmp( cmdStr, "rw" ) == 0 ))
{
uint8_t offset;
uint8_t numBytes;
uint8_t data[ 20 ];
if ( !ParseOffsetAndData( line, &offset, &numBytes, data, sizeof( data )))
{
return true;
}
m_device.SendDeferredWrite( offset, data, numBytes );
}
else
if ( strcmp( cmdStr, "get" ) == 0 )
{
ProcessDeviceGetCommand( devType, id, line, false );
}
else
if ( strcmp( cmdStr, "get-raw" ) == 0 )
{
ProcessDeviceGetCommand( devType, id, line, true );
}
else
if ( strcmp( cmdStr, "set" ) == 0 )
{
ProcessDeviceSetCommand( devType, id, line, false );
}
else
if ( strcmp( cmdStr, "set-raw" ) == 0 )
{
ProcessDeviceSetCommand( devType, id, line, true );
}
else
if ( strcmp( cmdStr, "reset" ) == 0 )
{
m_device.Reset();
}
else
{
LogError( "Unrecognized command: '%s'\n", cmdStr );
}
return true;
}