558 lines
15 KiB
C++
558 lines
15 KiB
C++
/******************************************************
|
|
* Rapid Embedded Development Architecture (redA) *
|
|
* Copyright (C) 2008 RecoSys *
|
|
* -------------- *
|
|
* TLiteProtcol: *
|
|
* Simplified protocol object to allow for fast *
|
|
* communications between componets *
|
|
* *
|
|
* Contents: *
|
|
* CLiteProtocol - Object Functions & Procedures *
|
|
* *
|
|
******************************************************/
|
|
|
|
using namespace std;
|
|
|
|
// LAStronic Libraries
|
|
#include "LiteProtocolCore.h"
|
|
|
|
// Standard C/C++ Libraries
|
|
#include <cstdlib>
|
|
#include <iostream>
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
|
|
//---------------------------------------------------------------------------
|
|
|
|
CLiteProtocol::CLiteProtocol( int pBufferInc, char pStartChar, char pEndChar, char pSeparator )
|
|
{
|
|
// Set Buffer increment
|
|
if (pBufferInc < 1)
|
|
BufferInc = 100;
|
|
else
|
|
BufferInc = pBufferInc;
|
|
|
|
// Set Protocol markers
|
|
StartChar = pStartChar;
|
|
EndChar = pEndChar;
|
|
Separator = pSeparator;
|
|
|
|
// Reset Buffer and protocol parameters
|
|
CommandStr = NULL;
|
|
BufferSize = 0;
|
|
ClearCommandStr();
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|
|
CLiteProtocol::~CLiteProtocol()
|
|
{
|
|
// Destroy string if exist
|
|
if (CommandStr)
|
|
free (CommandStr);
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|
|
bool CLiteProtocol::IncCommandStr( int pLength )
|
|
{
|
|
// Calculate new buffer length
|
|
pLength = BufferInc * (pLength/BufferInc + 1);
|
|
|
|
// Allocate memory
|
|
if (!CommandStr)
|
|
CommandStr = (char*)malloc(sizeof(char)*(pLength+1)); // Allow one more for zero termination
|
|
else if (BufferSize < pLength)
|
|
CommandStr = (char*)realloc(CommandStr, sizeof(char)*(pLength+1)); // Allow one more for zero termination
|
|
else
|
|
return true;
|
|
|
|
// Check if successful
|
|
if (!CommandStr) {
|
|
strcpy( Error, "Memory allocation error");
|
|
Verified = false;
|
|
return false;
|
|
}
|
|
|
|
// Update Buffer size and clear appended buffer
|
|
BufferSize = pLength;
|
|
memset( &CommandStr[Length], 0, pLength-Length+1 );
|
|
return true;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|
|
bool CLiteProtocol::UpdateCommandStr()
|
|
{
|
|
// Check if command sting exists
|
|
if (!CommandStr)
|
|
return false;
|
|
|
|
// Set Size & No of Params
|
|
sprintf( &CommandStr[1], "%06ld", Length );
|
|
CommandStr[7] = Separator;
|
|
sprintf( &CommandStr[8], "%04d", NoOfParams );
|
|
CommandStr[12] = Separator;
|
|
|
|
return true;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|
|
bool CLiteProtocol::VerifyStr( const char * pString, int pLength, int &ParamCount, char * pError )
|
|
{
|
|
char * Mark;
|
|
long tmpLen;
|
|
int tmpParams;
|
|
long Pos;
|
|
long ParamLen;
|
|
|
|
// Validate buffer
|
|
if (!pString) {
|
|
strcpy( pError, "NULL Protocol string" );
|
|
return false;
|
|
}
|
|
|
|
// Validate buffer
|
|
if (!pLength) {
|
|
strcpy( pError, "Empty Protocol string" );
|
|
return false;
|
|
}
|
|
|
|
// Verify Start and end
|
|
if (pString[0] != StartChar) {
|
|
sprintf( pError, "Invalid Start character - %02Xh", pString[0] );
|
|
return false;
|
|
}
|
|
|
|
// Verify Start and end
|
|
if (pString[pLength-1] != EndChar) {
|
|
sprintf( pError, "Invalid End character - %02Xh", pString[pLength-1] );
|
|
return false;
|
|
}
|
|
|
|
// Verify Length
|
|
Mark = (char*)memchr( &pString[1], Separator, 7 );
|
|
if ((Mark && (Mark-pString-1 != 6)) || ((tmpLen = atol(&pString[1])) != pLength)) {
|
|
strcpy( pError, "Invalid Protocol length" );
|
|
return false;
|
|
}
|
|
|
|
// Verify no of parameters
|
|
Mark = (char*)memchr( &pString[8], Separator, 5 );
|
|
if ((Mark && (Mark-pString-8 != 4)) || ((tmpParams = atoi(&pString[8])) < 1)) {
|
|
strcpy( pError, "Invalid Parameter count" );
|
|
return false;
|
|
};
|
|
|
|
// Count parameters
|
|
Pos = 13;
|
|
ParamCount = 0;
|
|
while ((pString[Pos] != EndChar) && (Pos < tmpLen-1))
|
|
{
|
|
// Get param length & move to next param
|
|
if (!(Mark = (char*)memchr( &pString[Pos], Separator, tmpLen-Pos )))
|
|
break;
|
|
ParamLen = Mark-pString-Pos;
|
|
Pos = Pos + ParamLen + 1;
|
|
ParamCount++;
|
|
};
|
|
|
|
// Check if match
|
|
if (ParamCount != tmpParams){
|
|
strcpy( pError, "Invalid No of Parameters" );
|
|
return false;
|
|
};
|
|
|
|
return true;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|
|
bool CLiteProtocol::ClearCommandStr()
|
|
{
|
|
// Set to empty buffers
|
|
if (CommandStr){
|
|
free( CommandStr );
|
|
CommandStr = NULL;
|
|
BufferSize = 0;
|
|
};
|
|
|
|
// Reset Parameters
|
|
Length = 0;
|
|
NoOfParams = 0;
|
|
|
|
// No string to verify
|
|
Verified = false;
|
|
strcpy( Error, "Empty String" );
|
|
|
|
// Set references to zero
|
|
CurrPos = 0;
|
|
CurrParam = 0;
|
|
|
|
return true;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|
|
bool CLiteProtocol::NewCommandStr()
|
|
{
|
|
// First clear existing command string
|
|
if (CommandStr){
|
|
free( CommandStr );
|
|
CommandStr = NULL;
|
|
BufferSize = 0;
|
|
Length = 0;
|
|
};
|
|
IncCommandStr( 14 );
|
|
|
|
// Build basic string
|
|
NoOfParams = 0;
|
|
Length = 14;
|
|
CommandStr[0] = StartChar;
|
|
CommandStr[13] = EndChar;
|
|
UpdateCommandStr();
|
|
|
|
// Not verified
|
|
Verified = false;
|
|
strcpy( Error, "No Parameters" );
|
|
|
|
// Set references
|
|
CurrParam = 1;
|
|
CurrPos = 13;
|
|
|
|
return true;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|
|
char* CLiteProtocol::GetCommandStr( char ** pString, int * pLength )
|
|
{
|
|
// Set return values
|
|
if (pString)
|
|
*pString = CommandStr;
|
|
if (pLength)
|
|
*pLength = Length;
|
|
|
|
return CommandStr;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|
|
bool CLiteProtocol::SetCommandStr( const char * pString, const int pLength )
|
|
{
|
|
int ParamCount;
|
|
|
|
// Verify new string
|
|
if (!VerifyStr( pString, pLength, ParamCount, Error ))
|
|
return false;
|
|
|
|
// Allocate memory
|
|
ClearCommandStr();
|
|
if (!IncCommandStr( pLength ))
|
|
return false;
|
|
|
|
// Copy string
|
|
memcpy( CommandStr, pString, pLength );
|
|
Length = pLength;
|
|
NoOfParams = ParamCount;
|
|
|
|
// Verified
|
|
Verified = true;
|
|
Error[0] = 0;
|
|
|
|
// Set all pointers to start position
|
|
GotoParam( 1 );
|
|
return true;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|
|
bool CLiteProtocol::AppendCommandStr( char * pString, int &pLength )
|
|
{
|
|
long Pos = 0;
|
|
long Start = pLength;
|
|
long End = pLength;
|
|
long newLen = 0;
|
|
|
|
// Check if valid parameters
|
|
if (!pString || (pLength < 1))
|
|
return false;
|
|
|
|
// Clear if last string is already verified
|
|
if (Verified)
|
|
ClearCommandStr();
|
|
|
|
// if partial string already received mark start position
|
|
if (Length != 0)
|
|
Start = 0;
|
|
|
|
// Look for other markers
|
|
while (Pos < pLength) {
|
|
// Check if start character
|
|
if (pString[Pos] == StartChar) {
|
|
// Mark start point
|
|
Start = Pos;
|
|
}
|
|
// Check if end character with valid start
|
|
else if ((pString[Pos] == EndChar) && (Start < pLength)) {
|
|
// Mark end and continue
|
|
End = Pos+1;
|
|
break;
|
|
}
|
|
|
|
// next char
|
|
Pos++;
|
|
};
|
|
|
|
// Check if anything available
|
|
if (Start < pLength) {
|
|
// Calculate length
|
|
newLen = End - Start;
|
|
|
|
// Increase buffer size
|
|
IncCommandStr( Length + newLen ); // negative value -> add length
|
|
|
|
// Copy buffer section
|
|
memcpy( &CommandStr[Length], &pString[Start], newLen );
|
|
Length = Length + newLen;
|
|
CommandStr[Length] = 0;
|
|
|
|
// Check if complete string
|
|
if ((pString[End-1] == EndChar))
|
|
VerifyCommandStr();
|
|
};
|
|
|
|
// Shift buffer
|
|
if (End < pLength)
|
|
memmove( pString, &pString[End], pLength-End );
|
|
memset( &pString[pLength-End], 0, End );
|
|
pLength = pLength - End;
|
|
|
|
return Verified;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|
|
bool CLiteProtocol::VerifyCommandStr()
|
|
{
|
|
// Check command string
|
|
if (VerifyStr( CommandStr, Length, NoOfParams, Error )) {
|
|
Verified = true;
|
|
Error[0] = 0;
|
|
}
|
|
|
|
// Move reference to first parameter
|
|
GotoParam( 1 );
|
|
return Verified;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|
|
bool CLiteProtocol::AppendParam( const char * pString, const int pLength )
|
|
{
|
|
int StringLen;
|
|
|
|
// Check if valid
|
|
if (!pLength || (!pString && (pLength == -1))) {
|
|
// Increase string length if required
|
|
StringLen = 0;
|
|
IncCommandStr( Length + 1 );
|
|
|
|
// Copy string
|
|
CommandStr[CurrPos+StringLen] = Separator;
|
|
Length += 1; // Allow for separator character
|
|
}
|
|
else {
|
|
// Increase string length if required
|
|
StringLen = (pLength < 0)? strlen(pString) : pLength;
|
|
IncCommandStr( Length + StringLen+1 );
|
|
|
|
// Copy string
|
|
memcpy( &CommandStr[CurrPos], (char*)pString, StringLen );
|
|
CommandStr[CurrPos+StringLen] = Separator;
|
|
Length = Length + StringLen + 1; // Allow for separator character
|
|
}
|
|
|
|
// Move pointer to end and set end char
|
|
CurrParam++;
|
|
CurrPos += StringLen + 1; // Allow for separator character
|
|
CommandStr[CurrPos] = EndChar;
|
|
|
|
// Update references
|
|
NoOfParams++;
|
|
UpdateCommandStr();
|
|
|
|
// String now verified - at least one parameter
|
|
Verified = true;
|
|
Error[0] = 0;
|
|
return true;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|
|
bool CLiteProtocol::AppendParam( long pValue )
|
|
{
|
|
char TempStr[10];
|
|
|
|
// Create string representation
|
|
sprintf( TempStr, "%ld", pValue );
|
|
|
|
// Insert
|
|
if (AppendParam( TempStr, strlen(TempStr) ))
|
|
return true;
|
|
else
|
|
return false;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|
|
bool CLiteProtocol::AppendParam( double pValue, int pDigits )
|
|
{
|
|
char ConvStr[10];
|
|
char TempStr[20];
|
|
|
|
// Check if command sting valid
|
|
if (!Verified)
|
|
return false;
|
|
|
|
// Create string representation
|
|
if (pDigits)
|
|
sprintf( ConvStr, "%%.%df", pDigits );
|
|
else
|
|
strcpy( ConvStr, "%f" );
|
|
sprintf( TempStr, ConvStr, pValue );
|
|
|
|
// Insert
|
|
if (AppendParam( TempStr, strlen(TempStr) ))
|
|
return true;
|
|
else
|
|
return false;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|
|
char const * CLiteProtocol::GetParamNo( int ParamNo, char ** pString, int * pLength )
|
|
{
|
|
char * Mark = NULL;
|
|
|
|
// Check if valid parameter no
|
|
if (!Verified || (ParamNo < 1) || (ParamNo > NoOfParams))
|
|
return NULL;
|
|
|
|
// Set Starting point
|
|
if (ParamNo < CurrParam)
|
|
GotoParam( 1 );
|
|
|
|
// Jump until parameter found
|
|
while ((CurrParam != ParamNo) && NextParam()) {};
|
|
|
|
// Check if found
|
|
if (CurrParam != ParamNo)
|
|
return NULL;
|
|
|
|
// Return reference to parameter
|
|
if (pString)
|
|
*pString = &CommandStr[CurrPos];
|
|
if (pLength) {
|
|
if (!(Mark = (char*)memchr( &CommandStr[CurrPos], Separator, Length-CurrPos )))
|
|
*pLength = 0;
|
|
else
|
|
*pLength = Mark - &CommandStr[CurrPos];
|
|
}
|
|
return (&CommandStr[CurrPos]);
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|
|
bool CLiteProtocol::GotoParam( int ParamNo )
|
|
{
|
|
// Check if command sting valid
|
|
if (!Verified || (ParamNo < 1) || (ParamNo > NoOfParams+1))
|
|
return false;
|
|
|
|
// Goto Param one
|
|
CurrPos = 13;
|
|
CurrParam = 1;
|
|
|
|
// Find Parameter
|
|
while ((CurrParam < ParamNo) && NextParam()) {};
|
|
|
|
return ((ParamNo == CurrParam)? true : false);
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|
|
bool CLiteProtocol::NextParam()
|
|
{
|
|
int ParamLen;
|
|
char * Mark = NULL;
|
|
|
|
// Check if command sting valid
|
|
if (!Verified || (CurrParam == NoOfParams))
|
|
return false;
|
|
|
|
// Get Current parameter length
|
|
if (!(Mark = (char*)memchr( &CommandStr[CurrPos], Separator, Length-CurrPos )))
|
|
return false;
|
|
ParamLen = Mark - &CommandStr[CurrPos];
|
|
|
|
// Move reference
|
|
CurrPos = CurrPos + ParamLen + 1;
|
|
CurrParam++;
|
|
|
|
return true;
|
|
}
|
|
|
|
//---------------------------------------------------------------------------
|
|
|
|
char const * CLiteProtocol::GetParam( char ** pString, int * pLength )
|
|
{
|
|
char * Mark = NULL;
|
|
|
|
// Check if valid parameter no
|
|
if (!Verified) {
|
|
if (pString) *pString = NULL;
|
|
if (pLength) pLength = 0;
|
|
return NULL;
|
|
};
|
|
|
|
// Return reference to parameter
|
|
if (pString)
|
|
*pString = &CommandStr[CurrPos];
|
|
if (pLength) {
|
|
if (!(Mark = (char*)memchr( &CommandStr[CurrPos], Separator, Length-CurrPos )))
|
|
*pLength = 0;
|
|
else
|
|
*pLength = Mark - &CommandStr[CurrPos];
|
|
}
|
|
return (&CommandStr[CurrPos]);
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|
|
// Create command
|
|
bool CLiteProtocol::CreateCommand( char const * From, char const * To, char const * Command )
|
|
{
|
|
// Create new command
|
|
if (!NewCommandStr())
|
|
return false;
|
|
|
|
// Insert Parameters in correct order
|
|
if (!AppendParam( From ) || !AppendParam( To ) || !AppendParam( Command )) {
|
|
ClearCommandStr();
|
|
return false;
|
|
};
|
|
|
|
// All is well
|
|
return true;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|
|
// Read command
|
|
bool CLiteProtocol::ReadCommand( char ** From, char ** To, char ** Command, int * DataParams,
|
|
int * FromLen, int * ToLen, int * CommandLen )
|
|
{
|
|
// Check if parameters available
|
|
if (!Verified || (NoOfParams < 3))
|
|
return false;
|
|
|
|
// Read parameters one by one
|
|
GotoParam( 1 );
|
|
GetParam( From, FromLen );
|
|
NextParam();
|
|
GetParam( To, ToLen );
|
|
NextParam();
|
|
GetParam( Command, CommandLen );
|
|
|
|
if (DataParams)
|
|
*DataParams = NoOfParams - 3;
|
|
|
|
return true;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
|