Пример #1
0
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);
}
Пример #2
0
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);
}
Пример #3
0
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);
	}
}
Пример #4
0
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);
}
Пример #5
0
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);
}
Пример #6
0
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__
}
Пример #7
0
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);
}
Пример #8
0
void SendData( const void *buf, size_t bytesToWrite )
{
    if ( gDebug )
    {
        DumpMem( "Send", 0, buf, bytesToWrite );
    }
    gSerialPort.Write( buf, bytesToWrite );

} // SendData
Пример #9
0
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);
}
Пример #10
0
void BioloidBus::WriteBuffer()
{
    if ( m_showPackets )
    {
        DumpMem( "W", 0, m_data, m_dataBytes );
    }

    WriteBufferedData( m_data, m_dataBytes );

    m_dataBytes = 0;
}
Пример #11
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);
}
Пример #12
0
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);
}
Пример #13
0
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__
}
Пример #14
0
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;
}
Пример #15
0
static void DumpVector(const void* b, int n, size_t size, DumpState* D)
{
    DumpInt(n,D);
    DumpMem(b,n,size,D);
}
Пример #16
0
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
Пример #17
0
/**
*   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
Пример #18
0
static void DumpChar(int y, DumpState* D)
{
 char x=(char)y;
 DumpMem(&x, 1, sizeof(x), D);
}
Пример #19
0
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);

}
Пример #20
0
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;
}